/*------------------------------------------------------------------------------
* rtkpos.c : precise positioning
*
*          Copyright (C) 2007-2020 by T.TAKASU, All rights reserved.
*
* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $
* history : 2007/01/12 1.0  new
*           2007/03/13 1.1  add slip detection by LLI flag
*           2007/04/18 1.2  add antenna pcv correction
*                           change rtkpos argin
*           2008/07/18 1.3  refactored
*           2009/01/02 1.4  modify rtk positioning api
*           2009/03/09 1.5  support glonass, gallileo and qzs
*           2009/08/27 1.6  fix bug on numerical exception
*           2009/09/03 1.7  add check of valid satellite number
*                           add check time sync for moving-base
*           2009/11/23 1.8  add api rtkopenstat(),rtkclosestat()
*                           add receiver h/w bias estimation
*                           add solution status output
*           2010/04/04 1.9  support ppp-kinematic and ppp-static modes
*                           support earth tide correction
*                           changed api:
*                               rtkpos()
*           2010/09/07 1.10 add elevation mask to hold ambiguity
*           2012/02/01 1.11 add extended receiver error model
*                           add glonass interchannel bias correction
*                           add slip detectior by L1-L5 gf jump
*                           output snr of rover receiver in residuals
*           2013/03/10 1.12 add otl and pole tides corrections
*           2014/05/26 1.13 support beidou and galileo
*                           add output of gal-gps and bds-gps time offset
*           2014/05/28 1.14 fix bug on memory exception with many sys and freq
*           2014/08/26 1.15 add functino to swap sol-stat file with keywords
*           2014/10/21 1.16 fix bug on beidou amb-res with pos2-bdsarmode=0
*           2014/11/08 1.17 fix bug on ar-degradation by unhealthy satellites
*           2015/03/23 1.18 residuals referenced to reference satellite
*           2015/05/20 1.19 no output solution status file with Q=0
*           2015/07/22 1.20 fix bug on base station position setting
*           2016/07/30 1.21 suppress single solution if !prcopt.outsingle
*                           fix bug on slip detection of backward filter
*           2016/08/20 1.22 fix bug on ddres() function
*           2018/10/10 1.13 support api change of satexclude()
*           2018/12/15 1.14 disable ambiguity resolution for gps-qzss
*           2019/08/19 1.15 fix bug on return value of resamb_LAMBDA()
*           2020/11/30 1.16 support of NavIC/IRNSS in API rtkpos()
*                           add detecting cycle slips by L1-Lx GF phase jump
*                           delete GLONASS IFB correction in ddres()
*                           use integer types in stdint.h
*-----------------------------------------------------------------------------*/
//#include "vld.h"
#include <stdarg.h>
#include "rtklib.h"
#include "../global/global.h"



/* constants/macros ----------------------------------------------------------*/

#define SQR(x)      ((x)*(x))
#define SQRT(x)     ((x)<=0.0||(x)!=(x)?0.0:sqrt(x))
#define MIN(x,y)    ((x)<=(y)?(x):(y))
#define ROUND(x)    (int)floor((x)+0.5)

#define VAR_POS     SQR(30.0) /* initial variance of receiver pos (m^2) */
#define VAR_VEL     SQR(10.0) /* initial variance of receiver vel ((m/s)^2) */
#define VAR_ACC     SQR(10.0) /* initial variance of receiver acc ((m/ss)^2) */
#define VAR_HWBIAS  SQR(1.0)  /* initial variance of h/w bias ((m/MHz)^2) */
#define VAR_GRA     SQR(0.001) /* initial variance of gradient (m^2) */
#define INIT_ZWD    0.15     /* initial zwd (m) */

#define PRN_HWBIAS  1E-6     /* process noise of h/w bias (m/MHz/sqrt(s)) */
#define GAP_RESION  120      /* gap to reset ionosphere parameters (epochs) */
#define MAXACC      30.0     /* max accel for doppler slip detection (m/s^2) */

#define VAR_HOLDAMB 0.001    /* constraint to hold ambiguity (cycle^2) */

#define TTOL_MOVEB  (1.0+2*DTTOL)
                             /* time sync tolerance for moving-baseline (s) */

/* number of parameters (pos,ionos,tropos,hw-bias,phase-bias,real,estimated) */
#define NF(opt)     ((opt)->ionoopt==IONOOPT_IFLC?1:(opt)->nf)
#define NP(opt)     ((opt)->dynamics==0?3:9)
#define NI(opt)     ((opt)->ionoopt!=IONOOPT_EST?0:MAXSAT)
#define NT(opt)     ((opt)->tropopt<TROPOPT_EST?0:((opt)->tropopt<TROPOPT_ESTG?2:6))
#define NL(opt)     ((opt)->glomodear!=2?0:NFREQGLO)
#define NB(opt)     ((opt)->mode<=PMODE_DGPS?0:MAXSAT*NF(opt))
#define NR(opt)     (NP(opt)+NI(opt)+NT(opt)+NL(opt))
#define NX(opt)     (NR(opt)+NB(opt)) // 9+0+0+0+()

/* state variable index */
#define II(s,opt)   (NP(opt)+(s)-1)                 /* ionos (s:satellite no) */
#define IT(r,opt)   (NP(opt)+NI(opt)+NT(opt)/2*(r)) /* tropos (r:0=rov,1:ref) */
#define IL(f,opt)   (NP(opt)+NI(opt)+NT(opt)+(f))   /* receiver h/w bias */
#define IB(s,f,opt) (NR(opt)+MAXSAT*(f)+(s)-1) /* phase bias (s:satno,f:freq) */

#define SAT(index, f, opt) (index - NR(opt) + 1 - MAXSAT*(f)) // sat number

/* global variables ----------------------------------------------------------*/
static int statlevel=0;          /* rtk status output level (0:off) */
static FILE *fp_stat=NULL;       /* rtk status file pointer */
static char file_stat[1024]="";  /* rtk status file original path */
static gtime_t time_stat={0};    /* rtk status file time */

/* open solution status file ---------------------------------------------------
* open solution status file and set output level
* args   : char     *file   I   rtk status file
*          int      level   I   rtk status level (0: off)
* return : status (1:ok,0:error)
* notes  : file can constain time keywords (%Y,%y,%m...) defined in reppath().
*          The time to replace keywords is based on UTC of CPU time.
* output : solution status file record format
*
*   $POS,week,tow,stat,posx,posy,posz,posxf,posyf,poszf
*          week/tow : gps week no/time of week (s)
*          stat     : solution status
*          posx/posy/posz    : position x/y/z ecef (m) float
*          posxf/posyf/poszf : position x/y/z ecef (m) fixed
*
*   $VELACC,week,tow,stat,vele,veln,velu,acce,accn,accu,velef,velnf,veluf,accef,accnf,accuf
*          week/tow : gps week no/time of week (s)
*          stat     : solution status
*          vele/veln/velu    : velocity e/n/u (m/s) float
*          acce/accn/accu    : acceleration e/n/u (m/s^2) float
*          velef/velnf/veluf : velocity e/n/u (m/s) fixed
*          accef/accnf/accuf : acceleration e/n/u (m/s^2) fixed
*
*   $CLK,week,tow,stat,clk1,clk2,clk3,clk4
*          week/tow : gps week no/time of week (s)
*          stat     : solution status
*          clk1     : receiver clock bias GPS (ns)
*          clk2     : receiver clock bias GLO-GPS (ns)
*          clk3     : receiver clock bias GAL-GPS (ns)
*          clk4     : receiver clock bias BDS-GPS (ns)
*
*   $ION,week,tow,stat,sat,az,el,ion,ion-fixed
*          week/tow : gps week no/time of week (s)
*          stat     : solution status
*          sat      : satellite id
*          az/el    : azimuth/elevation angle(deg)
*          ion      : vertical ionospheric delay L1 (m) float
*          ion-fixed: vertical ionospheric delay L1 (m) fixed
*
*   $TROP,week,tow,stat,rcv,ztd,ztdf
*          week/tow : gps week no/time of week (s)
*          stat     : solution status
*          rcv      : receiver (1:rover,2:base station)
*          ztd      : zenith total delay (m) float
*          ztdf     : zenith total delay (m) fixed
*
*   $HWBIAS,week,tow,stat,frq,bias,biasf
*          week/tow : gps week no/time of week (s)
*          stat     : solution status
*          frq      : frequency (1:L1,2:L2,...)
*          bias     : h/w bias coefficient (m/MHz) float
*          biasf    : h/w bias coefficient (m/MHz) fixed
*
*   $SAT,week,tow,sat,frq,az,el,resp,resc,vsat,snr,fix,slip,lock,outc,slipc,rejc
*          week/tow : gps week no/time of week (s)
*          sat/frq  : satellite id/frequency (1:L1,2:L2,...)
*          az/el    : azimuth/elevation angle (deg)
*          resp     : pseudorange residual (m)
*          resc     : carrier-phase residual (m)
*          vsat     : valid data flag (0:invalid,1:valid)
*          snr      : signal strength (dbHz)
*          fix      : ambiguity flag  (0:no data,1:float,2:fixed,3:hold,4:ppp)
*          slip     : cycle-slip flag (bit1:slip,bit2:parity unknown)
*          lock     : carrier-lock count
*          outc     : data outage count
*          slipc    : cycle-slip count
*          rejc     : data reject (outlier) count
*
*-----------------------------------------------------------------------------*/
extern int rtkopenstat(const char *file, int level)
{
    gtime_t time=utc2gpst(timeget());
    char path[1024];
    
    trace(3,"rtkopenstat: file=%s level=%d\n",file,level);
    
    if (level<=0) return 0;
    
    reppath(file,path,time,"","");
    
    if (!(fp_stat=fopen(path,"w"))) {
        trace(1,"rtkopenstat: file open error path=%s\n",path);
        return 0;
    }
    strcpy(file_stat,file);
    time_stat=time;
    statlevel=level;
    return 1;
}
/* close solution status file --------------------------------------------------
* close solution status file
* args   : none
* return : none
*-----------------------------------------------------------------------------*/
extern void rtkclosestat(void)
{
    trace(3,"rtkclosestat:\n");
    
    if (fp_stat) fclose(fp_stat);
    fp_stat=NULL;
    file_stat[0]='\0';
    statlevel=0;
}
/* write solution status to buffer -------------------------------------------*/
extern int rtkoutstat(rtk_t *rtk, char *buff)
{
    ssat_t *ssat;
    double tow,pos[3],vel[3],acc[3],vela[3]={0},acca[3]={0},xa[3];
    int i,j,week,est,nfreq,nf=NF(&rtk->opt);
    char id[32],*p=buff;
    
    if (rtk->sol.stat<=SOLQ_NONE) {
        return 0;
    }
    /* write ppp solution status to buffer */
    if (rtk->opt.mode>=PMODE_PPP_KINEMA) {
        return pppoutstat(rtk,buff);
    }
    est=rtk->opt.mode>=PMODE_DGPS;
    nfreq=est?nf:1;
    tow=time2gpst(rtk->sol.time,&week);
    
    /* receiver position */
    if (est) {
        for (i=0;i<3;i++) xa[i]=i<rtk->na?rtk->xa[i]:0.0;
        p+=sprintf(p,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow,
                   rtk->sol.stat,rtk->x[0],rtk->x[1],rtk->x[2],xa[0],xa[1],
                   xa[2]);
    }
    else {
        p+=sprintf(p,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow,
                   rtk->sol.stat,rtk->sol.rr[0],rtk->sol.rr[1],rtk->sol.rr[2],
                   0.0,0.0,0.0);
    }
    /* receiver velocity and acceleration */
    if (est&&rtk->opt.dynamics) {
        ecef2pos(rtk->sol.rr,pos);
        ecef2enu(pos,rtk->x+3,vel);
        ecef2enu(pos,rtk->x+6,acc);
        if (rtk->na>=6) ecef2enu(pos,rtk->xa+3,vela);
        if (rtk->na>=9) ecef2enu(pos,rtk->xa+6,acca);
        p+=sprintf(p,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
                   week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],acc[0],acc[1],
                   acc[2],vela[0],vela[1],vela[2],acca[0],acca[1],acca[2]);
    }
    else {
        ecef2pos(rtk->sol.rr,pos);
        ecef2enu(pos,rtk->sol.rr+3,vel);
        p+=sprintf(p,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
                   week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],
                   0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);
    }
    /* receiver clocks */
    p+=sprintf(p,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
               week,tow,rtk->sol.stat,1,rtk->sol.dtr[0]*1E9,rtk->sol.dtr[1]*1E9,
               rtk->sol.dtr[2]*1E9,rtk->sol.dtr[3]*1E9);
    
    /* ionospheric parameters */
    if (est&&rtk->opt.ionoopt==IONOOPT_EST) {
        for (i=0;i<MAXSAT;i++) {
            ssat=rtk->ssat+i;
            if (!ssat->vs) continue;
            satno2id(i+1,id);
            j=II(i+1,&rtk->opt);
            xa[0]=j<rtk->na?rtk->xa[j]:0.0;
            p+=sprintf(p,"$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n",week,tow,
                       rtk->sol.stat,id,ssat->azel[0]*R2D,ssat->azel[1]*R2D,
                       rtk->x[j],xa[0]);
        }
    }
    /* tropospheric parameters */
    if (est&&(rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG)) {
        for (i=0;i<2;i++) {
            j=IT(i,&rtk->opt);
            xa[0]=j<rtk->na?rtk->xa[j]:0.0;
            p+=sprintf(p,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,
                       rtk->sol.stat,i+1,rtk->x[j],xa[0]);
        }
    }
    /* receiver h/w bias */
    if (est&&rtk->opt.glomodear==2) {
        for (i=0;i<nfreq;i++) {
            j=IL(i,&rtk->opt);
            xa[0]=j<rtk->na?rtk->xa[j]:0.0;
            p+=sprintf(p,"$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,
                       rtk->sol.stat,i+1,rtk->x[j],xa[0]);
        }
    }
    return (int)(p-buff);
}
/* swap solution status file -------------------------------------------------*/
static void swapsolstat(void)
{
    gtime_t time=utc2gpst(timeget());
    char path[1024];
    
    if ((int)(time2gpst(time     ,NULL)/INT_SWAP_STAT)==
        (int)(time2gpst(time_stat,NULL)/INT_SWAP_STAT)) {
        return;
    }
    time_stat=time;
    
    if (!reppath(file_stat,path,time,"","")) {
        return;
    }
    if (fp_stat) fclose(fp_stat);
    
    if (!(fp_stat=fopen(path,"w"))) {
        trace(2,"swapsolstat: file open error path=%s\n",path);
        return;
    }
    trace(3,"swapsolstat: path=%s\n",path);
}
/* output solution status ----------------------------------------------------*/
static void outsolstat(rtk_t *rtk)
{
    ssat_t *ssat;
    double tow;
    char buff[MAXSOLMSG+1],id[32];
    int i,j,n,week,nfreq,nf=NF(&rtk->opt);
    
    if (statlevel<=0||!fp_stat||!rtk->sol.stat) return;
    
    trace(3,"outsolstat:\n");
    
    /* swap solution status file */
    swapsolstat();
    
    /* write solution status */
    n=rtkoutstat(rtk,buff); buff[n]='\0';
    
    fputs(buff,fp_stat);
    
    if (rtk->sol.stat==SOLQ_NONE||statlevel<=1) return;
    
    tow=time2gpst(rtk->sol.time,&week);
    nfreq=rtk->opt.mode>=PMODE_DGPS?nf:1;
    
    /* write residuals and status */
    for (i=0;i<MAXSAT;i++) {
        ssat=rtk->ssat+i;
        if (!ssat->vs) continue;
        satno2id(i+1,id);
        for (j=0;j<nfreq;j++) {
            fprintf(fp_stat,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.1f,%d,%d,%d,%d,%d,%d\n",
                    week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D,
                    ssat->resp[j],ssat->resc[j],ssat->vsat[j],
                    ssat->snr[j]*SNR_UNIT,ssat->fix[j],ssat->slip[j]&3,
                    ssat->lock[j],ssat->outc[j],ssat->slipc[j],ssat->rejc[j]);
        }
    }
}


/* save error message --------------------------------------------------------*/
static void errmsg(rtk_t *rtk, const char *format, ...)
{
    char buff[256],tstr[32];
    int n;
    va_list ap;
    time2str(rtk->sol.time,tstr,2);
    n=sprintf(buff,"%s: ",tstr+11);
    va_start(ap,format);
    n+=vsprintf(buff+n,format,ap);
    va_end(ap);
    n=n<MAXERRMSG-rtk->neb?n:MAXERRMSG-rtk->neb;
    memcpy(rtk->errbuf+rtk->neb,buff,n);
    rtk->neb+=n;
    trace(2,"%s",buff);
}
/* single-differenced observable ---------------------------------------------*/
static double sdobs(const obsd_t *obs, int i, int j, int k)
{
    double pi=(k<NFREQ)?obs[i].L[k]:obs[i].P[k-NFREQ];
    double pj=(k<NFREQ)?obs[j].L[k]:obs[j].P[k-NFREQ];
    return pi==0.0||pj==0.0?0.0:pi-pj;
}
/* single-differenced geometry-free linear combination of phase --------------*/
static double gfobs(const obsd_t *obs, int i, int j, int k, const nav_t *nav)
{
    double freq1,freq2,L1,L2;
    
    freq1=sat2freq(obs[i].sat,obs[i].code[0],nav);
    freq2=sat2freq(obs[i].sat,obs[i].code[k],nav);
    L1=sdobs(obs,i,j,0);
    L2=sdobs(obs,i,j,k);
    if (freq1==0.0||freq2==0.0||L1==0.0||L2==0.0) return 0.0;
    return L1*CLIGHT/freq1-L2*CLIGHT/freq2;
}
/* single-differenced measurement error variance -----------------------------*/
static double varerr(int sat, int sys, double el, double bl, double dt, int f,
                     const prcopt_t *opt)
{
    double a,b,c=opt->err[3]*bl/1E4,d=CLIGHT*opt->sclkstab*dt,fact=1.0;
    double sinel=sin(el);
    int nf=NF(opt);
    
    if (f>=nf) fact=opt->eratio[f-nf];
    if (fact<=0.0) fact=opt->eratio[0];
    fact*=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:EFACT_GPS);
    a=fact*opt->err[1];
    b=fact*opt->err[2];
    
    return 2.0*(opt->ionoopt==IONOOPT_IFLC?3.0:1.0)*(a*a+b*b/sinel/sinel+c*c)+d*d;
}
/* baseline length -----------------------------------------------------------*/
static double baseline(const double *ru, const double *rb, double *dr)
{
    int i;
    for (i=0;i<3;i++) dr[i]=ru[i]-rb[i];
    return norm(dr,3);
}
/* initialize state and covariance -------------------------------------------*/
static void initx(rtk_t *rtk, double xi, double var, int i)
{
    int j;
    rtk->x[i]=xi;
    for (j=0;j<rtk->nx;j++) {
        rtk->P[i+j*rtk->nx]=rtk->P[j+i*rtk->nx]=i==j?var:0.0;
    }
}
/* select common satellites between rover and reference station --------------*/
static int selsat(const obsd_t *obs, double *azel, int nu, int nr,
                  const prcopt_t *opt, int *sat, int *iu, int *ir)
{
    int i,j,k=0;
    
    trace(3,"selsat  : nu=%d nr=%d\n",nu,nr);
    
    for (i=0,j=nu;i<nu&&j<nu+nr;i++,j++) {
        if      (obs[i].sat<obs[j].sat) j--;
        else if (obs[i].sat>obs[j].sat) i--;
        else if (azel[1+j*2]>=opt->elmin) { /* elevation at base station */
            sat[k]=obs[i].sat; iu[k]=i; ir[k++]=j;
            trace(4,"(%2d) sat=%3d iu=%2d ir=%2d\n",k-1,obs[i].sat,i,j);
        }
    }
    return k;
}
/* temporal update of position/velocity/acceleration -------------------------*/
static void udpos(rtk_t *rtk, double tt)
{
    double *F,*P,*FP,*x,*xp,pos[3],Q[9]={0},Qv[9],var=0.0;
    int i,j,*ix,nx;
    
    trace(3,"udpos   : tt=%.3f\n",tt);
    
    /* fixed mode */
    if (rtk->opt.mode==PMODE_FIXED) {
        for (i=0;i<3;i++) initx(rtk,rtk->opt.ru[i],1E-8,i);
        return;
    }
    /* initialize position for first epoch */
    if (norm(rtk->x,3)<=0.0) {
        for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i);
        if (rtk->opt.dynamics) {
            for (i=3;i<6;i++) initx(rtk,rtk->sol.rr[i],VAR_VEL,i);
            for (i=6;i<9;i++) initx(rtk,1E-6,VAR_ACC,i);
        }
    }
    /* static mode */
    if (rtk->opt.mode==PMODE_STATIC) return;
    
    /* kinmatic mode without dynamics */
    if (!rtk->opt.dynamics) {
        for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i);
        return;
    }
    /* check variance of estimated postion */
    for (i=0;i<3;i++) var+=rtk->P[i+i*rtk->nx];
    var/=3.0;
    
    if (var>VAR_POS) {
        /* reset position with large variance */
        for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i);
        for (i=3;i<6;i++) initx(rtk,rtk->sol.rr[i],VAR_VEL,i);
        for (i=6;i<9;i++) initx(rtk,1E-6,VAR_ACC,i);
        trace(2,"reset rtk position due to large variance: var=%.3f\n",var);
        return;
    }
    /* generate valid state index */
    ix=imat(rtk->nx,1);
    for (i=nx=0;i<rtk->nx;i++) {
        if (rtk->x[i]!=0.0&&rtk->P[i+i*rtk->nx]>0.0) ix[nx++]=i;
    }
    if (nx<9) {
        free(ix);
        return;
    }
    /* state transition of position/velocity/acceleration */
    F=eye(nx); P=mat(nx,nx); FP=mat(nx,nx); x=mat(nx,1); xp=mat(nx,1);
    
    for (i=0;i<6;i++) {
        F[i+(i+3)*nx]=tt;
    }
    for (i=0;i<3;i++) {
        F[i+(i+6)*nx]=SQR(tt)/2.0;
    }
    for (i=0;i<nx;i++) {
        x[i]=rtk->x[ix[i]];
        for (j=0;j<nx;j++) {
            P[i+j*nx]=rtk->P[ix[i]+ix[j]*rtk->nx];
        }
    }
    //trace(4, "udpos: F = \n"); tracemat(4, F, nx, nx, 5, 8);
    /* x=F*x, P=F*P*F+Q */
    matmul("NN",nx,1,nx,1.0,F,x,0.0,xp);
    matmul("NN",nx,nx,nx,1.0,F,P,0.0,FP);
    matmul("NT",nx,nx,nx,1.0,FP,F,0.0,P);
    
    for (i=0;i<nx;i++) {
        rtk->x[ix[i]]=xp[i];
        for (j=0;j<nx;j++) {
            rtk->P[ix[i]+ix[j]*rtk->nx]=P[i+j*nx];
        }
    }
    /* process noise added to only acceleration */
    Q[0]=Q[4]=SQR(rtk->opt.prn[3])*fabs(tt);
    Q[8]=SQR(rtk->opt.prn[4])*fabs(tt);
    ecef2pos(rtk->x,pos);
    covecef(pos,Q,Qv);
    for (i=0;i<3;i++) for (j=0;j<3;j++) {
        rtk->P[i+6+(j+6)*rtk->nx]+=Qv[i+j*3];
    }
    free(ix); free(F); free(P); free(FP); free(x); free(xp);
}
/* temporal update of ionospheric parameters ---------------------------------*/
static void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns)
{
    double el,fact;
    int i,j;
    
    trace(3,"udion   : tt=%.3f bl=%.0f ns=%d\n",tt,bl,ns);
    
    for (i=1;i<=MAXSAT;i++) {
        j=II(i,&rtk->opt);
        if (rtk->x[j]!=0.0&&
            rtk->ssat[i-1].outc[0]>GAP_RESION&&rtk->ssat[i-1].outc[1]>GAP_RESION)
            rtk->x[j]=0.0;
    }
    for (i=0;i<ns;i++) {
        j=II(sat[i],&rtk->opt);
        
        if (rtk->x[j]==0.0) {
            initx(rtk,1E-6,SQR(rtk->opt.std[1]*bl/1E4),j);
        }
        else {
            /* elevation dependent factor of process noise */
            el=rtk->ssat[sat[i]-1].azel[1];
            fact=cos(el);
            rtk->P[j+j*rtk->nx]+=SQR(rtk->opt.prn[1]*bl/1E4*fact)*fabs(tt);
        }
    }
}
/* temporal update of tropospheric parameters --------------------------------*/
static void udtrop(rtk_t *rtk, double tt, double bl)
{
    int i,j,k;
    
    trace(3,"udtrop  : tt=%.3f\n",tt);
    
    for (i=0;i<2;i++) {
        j=IT(i,&rtk->opt);
        
        if (rtk->x[j]==0.0) {
            initx(rtk,INIT_ZWD,SQR(rtk->opt.std[2]),j); /* initial zwd */
            
            if (rtk->opt.tropopt>=TROPOPT_ESTG) {
                for (k=0;k<2;k++) initx(rtk,1E-6,VAR_GRA,++j);
            }
        }
        else {
            rtk->P[j+j*rtk->nx]+=SQR(rtk->opt.prn[2])*fabs(tt);
            
            if (rtk->opt.tropopt>=TROPOPT_ESTG) {
                for (k=0;k<2;k++) {
                    rtk->P[++j*(1+rtk->nx)]+=SQR(rtk->opt.prn[2]*0.3)*fabs(tt);
                }
            }
        }
    }
}
/* temporal update of receiver h/w biases ------------------------------------*/
static void udrcvbias(rtk_t *rtk, double tt)
{
    int i,j;
    
    trace(3,"udrcvbias: tt=%.3f\n",tt);
    
    for (i=0;i<NFREQGLO;i++) {
        j=IL(i,&rtk->opt);
        
        if (rtk->x[j]==0.0) {
            initx(rtk,1E-6,VAR_HWBIAS,j);
        }
        /* hold to fixed solution */
        else if (rtk->nfix>=rtk->opt.minfix&&rtk->sol.ratio>rtk->opt.thresar[0]) {
            initx(rtk,rtk->xa[j],rtk->Pa[j+j*rtk->na],j);
        }
        else {
            rtk->P[j+j*rtk->nx]+=SQR(PRN_HWBIAS)*fabs(tt);
        }
    }
}
/* detect cycle slip by LLI --------------------------------------------------*/
static void detslp_ll(rtk_t *rtk, const obsd_t *obs, int i, int rcv)
{
    uint32_t slip,LLI;
    int f,sat=obs[i].sat;
    
    trace(3,"detslp_ll: i=%d rcv=%d\n",i,rcv);
    
    for (f=0;f<rtk->opt.nf;f++) {
        
        if (obs[i].L[f]==0.0||
            fabs(timediff(obs[i].time,rtk->ssat[sat-1].pt[rcv-1][f]))<DTTOL) {
            continue;
        }
        /* restore previous LLI */
        if (rcv==1) LLI=getbitu(&rtk->ssat[sat-1].slip[f],0,2); /* rover */
        else        LLI=getbitu(&rtk->ssat[sat-1].slip[f],2,2); /* base  */
        
        /* detect slip by cycle slip flag in LLI */
        if (rtk->tt>=0.0) { /* forward */
            if (obs[i].LLI[f]&1) {
                errmsg(rtk,"slip detected forward  (sat=%2d rcv=%d F=%d LLI=%x)\n",
                       sat,rcv,f+1,obs[i].LLI[f]);
            }
            slip=obs[i].LLI[f];
        }
        else { /* backward */
            if (LLI&1) {
                errmsg(rtk,"slip detected backward (sat=%2d rcv=%d F=%d LLI=%x)\n",
                       sat,rcv,f+1,LLI);
            }
            slip=LLI;
        }
        /* detect slip by parity unknown flag transition in LLI */
        if (((LLI&2)&&!(obs[i].LLI[f]&2))||(!(LLI&2)&&(obs[i].LLI[f]&2))) {
            errmsg(rtk,"slip detected half-cyc (sat=%2d rcv=%d F=%d LLI=%x->%x)\n",
                   sat,rcv,f+1,LLI,obs[i].LLI[f]);
            slip|=1;
        }
        /* save current LLI */
        if (rcv==1) setbitu(&rtk->ssat[sat-1].slip[f],0,2,obs[i].LLI[f]);
        else        setbitu(&rtk->ssat[sat-1].slip[f],2,2,obs[i].LLI[f]);
        
        /* save slip and half-cycle valid flag */
        rtk->ssat[sat-1].slip[f]|=(uint8_t)slip;
        rtk->ssat[sat-1].half[f]=(obs[i].LLI[f]&2)?0:1;
    }
}
/* detect cycle slip by geometry free phase jump -----------------------------*/
static void detslp_gf(rtk_t *rtk, const obsd_t *obs, int i, int j,
                      const nav_t *nav)
{
    int k,sat=obs[i].sat;
    double g0,g1;
    
    trace(3,"detslp_gf: i=%d j=%d\n",i,j);
    
    for (k=1;k<rtk->opt.nf;k++) {
        if ((g1=gfobs(obs,i,j,k,nav))==0.0) return;
         
        g0=rtk->ssat[sat-1].gf[k-1];
        rtk->ssat[sat-1].gf[k-1]=g1;
         
        if (g0!=0.0&&fabs(g1-g0)>rtk->opt.thresslip) {
            rtk->ssat[sat-1].slip[0]|=1;
            rtk->ssat[sat-1].slip[k]|=1;
            errmsg(rtk,"slip detected GF jump (sat=%2d L1-L%d GF=%.3f %.3f)\n",
                   sat,k+1,g0,g1);
        }
    }
}
/* detect cycle slip by doppler and phase difference -------------------------*/
static void detslp_dop(rtk_t *rtk, const obsd_t *obs, int i, int rcv,
                       const nav_t *nav)
{
#if 0 /* detection with doppler disabled because of clock-jump issue (v.2.3.0) */
    int f,sat=obs[i].sat;
    double tt,dph,dpt,lam,thres;
    
    trace(3,"detslp_dop: i=%d rcv=%d\n",i,rcv);
    
    for (f=0;f<rtk->opt.nf;f++) {
        if (obs[i].L[f]==0.0||obs[i].D[f]==0.0||rtk->ph[rcv-1][sat-1][f]==0.0) {
            continue;
        }
        if (fabs(tt=timediff(obs[i].time,rtk->pt[rcv-1][sat-1][f]))<DTTOL) continue;
        if ((lam=nav->lam[sat-1][f])<=0.0) continue;
        
        /* cycle slip threshold (cycle) */
        thres=MAXACC*tt*tt/2.0/lam+rtk->opt.err[4]*fabs(tt)*4.0;
        
        /* phase difference and doppler x time (cycle) */
        dph=obs[i].L[f]-rtk->ph[rcv-1][sat-1][f];
        dpt=-obs[i].D[f]*tt;
        
        if (fabs(dph-dpt)<=thres) continue;
        
        rtk->slip[sat-1][f]|=1;
        
        errmsg(rtk,"slip detected (sat=%2d rcv=%d L%d=%.3f %.3f thres=%.3f)\n",
               sat,rcv,f+1,dph,dpt,thres);
    }
#endif
}
/* temporal update of phase biases -------------------------------------------*/
static void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat,
                   const int *iu, const int *ir, int ns, const nav_t *nav)
{
    double cp,pr,cp1,cp2,pr1,pr2,*bias,offset,freqi,freq1,freq2,C1,C2;
    int i,j,k,slip,reset,nf=NF(&rtk->opt);
    
    trace(3,"udbias  : tt=%.3f ns=%d\n",tt,ns);
    
    for (i=0;i<ns;i++) { 
        
        /* detect cycle slip by LLI */
        for (k=0;k<rtk->opt.nf;k++) rtk->ssat[sat[i]-1].slip[k]&=0xFC; 
        detslp_ll(rtk,obs,iu[i],1);
        detslp_ll(rtk,obs,ir[i],2);
        
        /* detect cycle slip by geometry-free phase jump */
        detslp_gf(rtk,obs,iu[i],ir[i],nav);
        
        /* detect cycle slip by doppler and phase difference */
        detslp_dop(rtk,obs,iu[i],1,nav);
        detslp_dop(rtk,obs,ir[i],2,nav);
        
        /* update half-cycle valid flag */
        for (k=0;k<nf;k++) {
            rtk->ssat[sat[i]-1].half[k]=
                !((obs[iu[i]].LLI[k]&2)||(obs[ir[i]].LLI[k]&2));
        }
    }
    for (k=0;k<nf;k++) {
        /* reset phase-bias if instantaneous AR or expire obs outage counter */
        for (i=1;i<=MAXSAT;i++) {
            
            reset=++rtk->ssat[i-1].outc[k]>(uint32_t)rtk->opt.maxout;
            
            if (rtk->opt.modear==ARMODE_INST&&rtk->x[IB(i,k,&rtk->opt)]!=0.0) {
                initx(rtk,0.0,0.0,IB(i,k,&rtk->opt)); 
            }
            else if (reset&&rtk->x[IB(i,k,&rtk->opt)]!=0.0) {
                initx(rtk,0.0,0.0,IB(i,k,&rtk->opt)); 
                trace(3,"udbias : obs outage counter overflow (sat=%3d L%d n=%d)\n",
                      i,k+1,rtk->ssat[i-1].outc[k]);
                rtk->ssat[i-1].outc[k]=0;
            }
            if (rtk->opt.modear!=ARMODE_INST&&reset) {
                rtk->ssat[i-1].lock[k]=-rtk->opt.minlock; 
            }
        }
        /* reset phase-bias if detecting cycle slip */
        for (i=0;i<ns;i++) {
            j=IB(sat[i],k,&rtk->opt);
            rtk->P[j+j*rtk->nx]+=rtk->opt.prn[0]*rtk->opt.prn[0]*fabs(tt);
            slip=rtk->ssat[sat[i]-1].slip[k];
            if (rtk->opt.ionoopt==IONOOPT_IFLC) slip|=rtk->ssat[sat[i]-1].slip[1];
            if (rtk->opt.modear==ARMODE_INST||!(slip&1)) continue;
            rtk->x[j]=0.0;
            rtk->ssat[sat[i]-1].lock[k]=-rtk->opt.minlock; 
        }
        bias=zeros(ns,1);
        
        /* estimate approximate phase-bias by phase-code */
        for (i=j=0,offset=0.0;i<ns;i++) {
            
            if (rtk->opt.ionoopt!=IONOOPT_IFLC) {
                cp=sdobs(obs,iu[i],ir[i],k); /* cycle */ 
                pr=sdobs(obs,iu[i],ir[i],k+NFREQ);
                freqi=sat2freq(sat[i],obs[iu[i]].code[k],nav);
                if (cp==0.0||pr==0.0||freqi==0.0) continue;
                
                bias[i]=cp-pr*freqi/CLIGHT; 
            }
            else {
                cp1=sdobs(obs,iu[i],ir[i],0);
                cp2=sdobs(obs,iu[i],ir[i],1);
                pr1=sdobs(obs,iu[i],ir[i],NFREQ);
                pr2=sdobs(obs,iu[i],ir[i],NFREQ+1);
                freq1=sat2freq(sat[i],obs[iu[i]].code[0],nav);
                freq2=sat2freq(sat[i],obs[iu[i]].code[1],nav);
                if (cp1==0.0||cp2==0.0||pr1==0.0||pr2==0.0||freq1==0.0||freq2<=0.0) continue;
                
                C1= SQR(freq1)/(SQR(freq1)-SQR(freq2));
                C2=-SQR(freq2)/(SQR(freq1)-SQR(freq2));
                bias[i]=(C1*cp1*CLIGHT/freq1+C2*cp2*CLIGHT/freq2)-(C1*pr1+C2*pr2);
            }
            if (rtk->x[IB(sat[i],k,&rtk->opt)]!=0.0) { 
                offset+=bias[i]-rtk->x[IB(sat[i],k,&rtk->opt)];
                j++; 
            }
        }
        /* correct phase-bias offset to enssure phase-code coherency */
        if (j>0) {
            for (i=1;i<=MAXSAT;i++) {
                if (rtk->x[IB(i,k,&rtk->opt)]!=0.0) rtk->x[IB(i,k,&rtk->opt)]+=offset/j; 
            }
        }
        /* set initial states of phase-bias */
        for (i=0;i<ns;i++) {
            if (bias[i]==0.0||rtk->x[IB(sat[i],k,&rtk->opt)]!=0.0) continue; 
            initx(rtk,bias[i],SQR(rtk->opt.std[0]),IB(sat[i],k,&rtk->opt));
        }
        free(bias);
    }
}
/* temporal update of states --------------------------------------------------*/
static void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat,
                    const int *iu, const int *ir, int ns, const nav_t *nav)
{
    double tt=rtk->tt,bl,dr[3];
    
    trace(3,"udstate : ns=%d\n",ns);
    
    /* temporal update of position/velocity/acceleration */
    udpos(rtk,tt);
    
    /* temporal update of ionospheric parameters */
    if (rtk->opt.ionoopt>=IONOOPT_EST) {
        bl=baseline(rtk->x,rtk->rb,dr);
        udion(rtk,tt,bl,sat,ns);
    }
    /* temporal update of tropospheric parameters */
    if (rtk->opt.tropopt>=TROPOPT_EST) {
        udtrop(rtk,tt,bl);
    }
    /* temporal update of eceiver h/w bias */
    if (rtk->opt.glomodear==2&&(rtk->opt.navsys&SYS_GLO)) {
        udrcvbias(rtk,tt);
    }
    /* temporal update of phase-bias */
    if (rtk->opt.mode>PMODE_DGPS) {
        udbias(rtk,tt,obs,sat,iu,ir,ns,nav);
    }
}
/* UD (undifferenced) phase/code residual for satellite ----------------------*/
static void zdres_sat(int base, double r, const obsd_t *obs, const nav_t *nav,
                      const double *azel, const double *dant,
                      const prcopt_t *opt, double *y, double *freq)
{
    double freq1,freq2,C1,C2,dant_if;
    int i,nf=NF(opt);
    
    if (opt->ionoopt==IONOOPT_IFLC) { /* iono-free linear combination */
        freq1=sat2freq(obs->sat,obs->code[0],nav);
        freq2=sat2freq(obs->sat,obs->code[1],nav);
        if (freq1==0.0||freq2==0.0) return;
        
        if (testsnr(base,0,azel[1],obs->SNR[0]*SNR_UNIT,&opt->snrmask)||
            testsnr(base,1,azel[1],obs->SNR[1]*SNR_UNIT,&opt->snrmask)) return;
        
        C1= SQR(freq1)/(SQR(freq1)-SQR(freq2));
        C2=-SQR(freq2)/(SQR(freq1)-SQR(freq2));
        dant_if=C1*dant[0]+C2*dant[1];
        
        if (obs->L[0]!=0.0&&obs->L[1]!=0.0) {
            y[0]=C1*obs->L[0]*CLIGHT/freq1+C2*obs->L[1]*CLIGHT/freq2-r-dant_if;
        }
        if (obs->P[0]!=0.0&&obs->P[1]!=0.0) {
            y[1]=C1*obs->P[0]+C2*obs->P[1]-r-dant_if;
        }
        freq[0]=1.0;
    }
    else {
        for (i=0;i<nf;i++) {
            if ((freq[i]=sat2freq(obs->sat,obs->code[i],nav))==0.0) continue;
            
            /* check SNR mask */
            if (testsnr(base,i,azel[1],obs->SNR[i]*SNR_UNIT,&opt->snrmask)) {
                continue;
            }
            /* residuals = observable - pseudorange */
            if (obs->L[i]!=0.0) y[i   ]=obs->L[i]*CLIGHT/freq[i]-r-dant[i];
            if (obs->P[i]!=0.0) y[i+nf]=obs->P[i]               -r-dant[i];
        }
    }
}
/* UD (undifferenced) phase/code residuals -----------------------------------
calculate zero diff residuals [observed pseudorange - range]
        output is in y[0:nu-1], only shared input with base is nav
 args:  I   base:  0=rover,1=base
        I   obs  = sat observations
        I   n    = # of sats
        I   rs [(0:2)+i*6]= sat position {x,y,z} (m)
        I   dts[(0:1)+i*2]= sat clock {bias,drift} (s|s/s)
        I   svh  = sat health flags
        I   nav  = sat nav data
        I   rr   = rcvr pos (x,y,z)
        I   opt  = options
        I   index: 0=base,1=rover
        O   y[(0:1)+i*2] = zero diff residuals {phase,code} (m)
        O   e    = line of sight unit vectors to sats
        O   azel = [az, el] to sats 
        O   freq: the carrier frequency         */
int zdres(int base, const obsd_t *obs, int n, const double *rs,
                 const double *dts, const double *var, const int *svh,
                 const nav_t *nav, const double *rr, const prcopt_t *opt,
                 int index, double *y, double *e, double *azel, double *freq)
{
    double r,rr_[3],pos[3],dant[NFREQ]={0},disp[3];
    double zhd,zazel[]={0.0,90.0*D2R};
    int i,nf=NF(opt); 
    
    if (n > 1) trace(2,"zdres   : n=%d\n",n);
    
    for (i=0;i<n*nf*2;i++) y[i]=0.0;
    
    if (norm(rr,3)<=0.0) return 0; /* no receiver position */
    
    for (i=0;i<3;i++) rr_[i]=rr[i]; 
    
    /* earth tide correction */
    if (opt->tidecorr) {
        tidedisp(gpst2utc(obs[0].time),rr_,opt->tidecorr,&nav->erp,
                 opt->odisp[base],disp);
        for (i=0;i<3;i++) rr_[i]+=disp[i];
    }
    ecef2pos(rr_,pos); 
    
    for (i=0;i<n;i++) {
        /* compute geometric-range and azimuth/elevation angle */
        if ((r=geodist(rs+i*6,rr_,e+i*3))<=0.0) continue; 
        if (satazel(pos,e+i*3,azel+i*2)<opt->elmin) continue; 
        
        /* excluded satellite? */
        if (satexclude(obs[i].sat,var[i],svh[i],opt)) continue;
        
        /* satellite clock-bias */
        r+=-CLIGHT*dts[i*2]; 
        
        /* troposphere delay model (hydrostatic) */
        zhd=tropmodel(obs[0].time,pos,zazel,0.0);
        r+=tropmapf(obs[i].time,pos,azel+i*2,NULL)*zhd;
        
        /* receiver antenna phase center correction */
        antmodel(opt->pcvr+index,opt->antdel[index],azel+i*2,opt->posopt[1],
                 dant);
        
        /* UD phase/code residual for satellite */
        zdres_sat(base,r,obs+i,nav,azel+i*2,dant,opt,y+i*nf*2,freq+i*nf);
    }
    return 1;
}
/* test valid observation data -----------------------------------------------*/
static int validobs(int i, int j, int f, int nf, double *y)
{
    /* if no phase observable, psudorange is also unusable */
    return y[f+i*nf*2]!=0.0&&y[f+j*nf*2]!=0.0&&
           (f<nf||(y[f-nf+i*nf*2]!=0.0&&y[f-nf+j*nf*2]!=0.0));
}
/* DD (double-differenced) measurement error covariance ----------------------*/
static void ddcov(const int *nb, int n, const double *Ri, const double *Rj,
                  int nv, double *R)
{
    int i,j,k=0,b;
    
    trace(3,"ddcov   : n=%d\n",n);
    
    for (i=0;i<nv*nv;i++) R[i]=0.0;
    for (b=0;b<n;k+=nb[b++]) {
        for (i=0;i<nb[b];i++) for (j=0;j<nb[b];j++) {
            R[k+i+(k+j)*nv]=Ri[k+i]+(i==j?Rj[k+i]:0.0);
        }
    }
    trace(4,"R=\n"); tracemat(4,R,nv,nv,8,6);
}
/* baseline length constraint ------------------------------------------------*/
static int constbl(rtk_t *rtk, const double *x, const double *P, double *v,
                   double *H, double *Ri, double *Rj, int index)
{
    const double thres=0.1; /* threshold for nonliearity (v.2.3.0) */
    double xb[3],b[3],bb,var=0.0;
    int i;
     
    trace(3,"constbl : \n");
    
    /* no constraint */
    if (rtk->opt.baseline[0]<=0.0) return 0;
    
    /* time-adjusted baseline vector and length */
    for (i=0;i<3;i++) {
        xb[i]=rtk->rb[i];
        b[i]=x[i]-xb[i];
    }
    bb=norm(b,3);
    
    /* approximate variance of solution */
    if (P) {
        for (i=0;i<3;i++) var+=P[i+i*rtk->nx];
        var/=3.0;
    }
    /* check nonlinearity */
    if (var>SQR(thres*bb)) {
        trace(3,"constbl : equation nonlinear (bb=%.3f var=%.3f)\n",bb,var);
        return 0;
    }
    /* constraint to baseline length */
    v[index]=rtk->opt.baseline[0]-bb;
    if (H) {
        for (i=0;i<3;i++) H[i+index*rtk->nx]=b[i]/bb;
    }
    Ri[index]=0.0;
    Rj[index]=SQR(rtk->opt.baseline[1]);
    
    trace(4,"baseline len   v=%13.3f R=%8.6f %8.6f\n",v[index],Ri[index],Rj[index]);
    
    return 1;
}
/* precise tropspheric model -------------------------------------------------*/
static double prectrop(gtime_t time, const double *pos, int r,
                       const double *azel, const prcopt_t *opt, const double *x,
                       double *dtdx)
{
    double m_w=0.0,cotz,grad_n,grad_e;
    int i=IT(r,opt);
    
    /* wet mapping function */
    tropmapf(time,pos,azel,&m_w);
    
    if (opt->tropopt>=TROPOPT_ESTG&&azel[1]>0.0) {
        
        /* m_w=m_0+m_0*cot(el)*(Gn*cos(az)+Ge*sin(az)): ref [6] */
        cotz=1.0/tan(azel[1]);
        grad_n=m_w*cotz*cos(azel[0]);
        grad_e=m_w*cotz*sin(azel[0]);
        m_w+=grad_n*x[i+1]+grad_e*x[i+2];
        dtdx[1]=grad_n*x[i];
        dtdx[2]=grad_e*x[i];
    }
    else dtdx[1]=dtdx[2]=0.0;
    dtdx[0]=m_w;
    return m_w*x[i];
}
/* test satellite system (m=0:GPS/SBS,1:GLO,2:GAL,3:BDS,4:QZS,5:IRN) ---------*/
static int test_sys(int sys, int m)
{
    switch (sys) {
        case SYS_GPS: return m==0;
        case SYS_SBS: return m==0;
        case SYS_GLO: return m==1;
        case SYS_GAL: return m==2;
        case SYS_CMP: return m==3;
        case SYS_QZS: return m==4;
        case SYS_IRN: return m==5;
    }
    return 0;
}
/* DD (double-differenced) phase/code residuals ------------------------------*/
static int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x,
                 const double *P, const int *sat, double *y, double *e,
                 double *azel, double *freq, const int *iu, const int *ir,
                 int ns, double *v, double *H, double *R, int *vflg)
{
    prcopt_t *opt=&rtk->opt;
    double bl,dr[3],posu[3],posr[3],didxi=0.0,didxj=0.0,*im;
    double *tropr,*tropu,*dtdxr,*dtdxu,*Ri,*Rj,freqi,freqj,*Hi=NULL;
    int i,j,k,m,f,nv=0,nb[2*(NFREQ*4*2+2)]={0},b=0,sysi,sysj,nf=NF(opt);
    
	// add by Qijia Zhao
	ddmeas_init(rtk);
	// end
    
    trace(2,"ddres   : dt=%.1f nx=%d ns=%d\n",dt,rtk->nx,ns);
    
    bl=baseline(x,rtk->rb,dr);
    ecef2pos(x,posu); ecef2pos(rtk->rb,posr);
    
    Ri=mat(ns*nf*2+2,1); Rj=mat(ns*nf*2+2,1); im=mat(ns,1);
    tropu=mat(ns,1); tropr=mat(ns,1); dtdxu=mat(ns,3); dtdxr=mat(ns,3);
    
    for (i=0;i<MAXSAT;i++) for (j=0;j<NFREQ;j++) {
        rtk->ssat[i].resp[j]=rtk->ssat[i].resc[j]=0.0;
    }
    /* compute factors of ionospheric and tropospheric delay */
    for (i=0;i<ns;i++) {
        if (opt->ionoopt>=IONOOPT_EST) {
            im[i]=(ionmapf(posu,azel+iu[i]*2)+ionmapf(posr,azel+ir[i]*2))/2.0;
        }
        if (opt->tropopt>=TROPOPT_EST) {
            tropu[i]=prectrop(rtk->sol.time,posu,0,azel+iu[i]*2,opt,x,dtdxu+i*3);
            tropr[i]=prectrop(rtk->sol.time,posr,1,azel+ir[i]*2,opt,x,dtdxr+i*3);
        }
    }
    for (m=0;m<6;m++) /* m=0:GPS/SBS,1:GLO,2:GAL,3:BDS,4:QZS,5:IRN */
    
    for (f=opt->mode>PMODE_DGPS?0:nf;f<nf*2;f++) {
        
        /* search reference satellite with highest elevation */
        for (i=-1,j=0;j<ns;j++) {
            sysi=rtk->ssat[sat[j]-1].sys;
            if (!test_sys(sysi,m)) continue;
            if (!validobs(iu[j],ir[j],f,nf,y)) continue;
            if (i<0||azel[1+iu[j]*2]>=azel[1+iu[i]*2]) i=j;
        } 
        if (i<0) continue;
        
        /* make DD (double difference) */
        for (j=0;j<ns;j++) {
            if (i==j) continue;
            sysi=rtk->ssat[sat[i]-1].sys; 
            sysj=rtk->ssat[sat[j]-1].sys;
            freqi=freq[f%nf+iu[i]*nf];
            freqj=freq[f%nf+iu[j]*nf];
            if (!test_sys(sysj,m)) continue; 
            if (!validobs(iu[j],ir[j],f,nf,y)) continue;
            
            if (H) {
                Hi=H+nv*rtk->nx; 
                for (k=0;k<rtk->nx;k++) Hi[k]=0.0;
            }
            /* DD residual */ 
            v[nv]=(y[f+iu[i]*nf*2]-y[f+ir[i]*nf*2])-
                  (y[f+iu[j]*nf*2]-y[f+ir[j]*nf*2]);
            
            /* partial derivatives by rover position */
            if (H) {
                for (k=0;k<3;k++) {
                    Hi[k]=-e[k+iu[i]*3]+e[k+iu[j]*3];
                }
            }
            /* DD ionospheric delay term */
            if (opt->ionoopt==IONOOPT_EST) {
                didxi=(f<nf?-1.0:1.0)*im[i]*SQR(FREQ1/freqi);
                didxj=(f<nf?-1.0:1.0)*im[j]*SQR(FREQ1/freqj);
                v[nv]-=didxi*x[II(sat[i],opt)]-didxj*x[II(sat[j],opt)];
                if (H) {
                    Hi[II(sat[i],opt)]= didxi;
                    Hi[II(sat[j],opt)]=-didxj;
                }
            }
            /* DD tropospheric delay term */
            if (opt->tropopt==TROPOPT_EST||opt->tropopt==TROPOPT_ESTG) {
                v[nv]-=(tropu[i]-tropu[j])-(tropr[i]-tropr[j]);
                for (k=0;k<(opt->tropopt<TROPOPT_ESTG?1:3);k++) {
                    if (!H) continue;
                    Hi[IT(0,opt)+k]= (dtdxu[k+i*3]-dtdxu[k+j*3]);
                    Hi[IT(1,opt)+k]=-(dtdxr[k+i*3]-dtdxr[k+j*3]);
                }
            }
            /* DD phase-bias term */
            if (f<nf) {
                if (opt->ionoopt!=IONOOPT_IFLC) {
                    v[nv]-=CLIGHT/freqi*x[IB(sat[i],f,opt)]-
                           CLIGHT/freqj*x[IB(sat[j],f,opt)];
                    if (H) {
                        Hi[IB(sat[i],f,opt)]= CLIGHT/freqi;
                        Hi[IB(sat[j],f,opt)]=-CLIGHT/freqj;
                    }
                }
                else {
                    v[nv]-=x[IB(sat[i],f,opt)]-x[IB(sat[j],f,opt)];
					
                    if (H) {
                        Hi[IB(sat[i],f,opt)]= 1.0;
                        Hi[IB(sat[j],f,opt)]=-1.0;
                    }
                }
            }
            if (f<nf) rtk->ssat[sat[j]-1].resc[f   ]=v[nv];
            else      rtk->ssat[sat[j]-1].resp[f-nf]=v[nv];
            
            /* test innovation */
            if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) {
                if (f<nf) {
                    rtk->ssat[sat[i]-1].rejc[f]++;
                    rtk->ssat[sat[j]-1].rejc[f]++;
                }
                errmsg(rtk,"outlier rejected (sat=%3d-%3d %s%d v=%.3f)\n",
                       sat[i],sat[j],f<nf?"L":"P",f%nf+1,v[nv]);
                continue;
            }
            /* SD (single-differenced) measurement error variances */
            Ri[nv]=varerr(sat[i],sysi,azel[1+iu[i]*2],bl,dt,f,opt);
            Rj[nv]=varerr(sat[j],sysj,azel[1+iu[j]*2],bl,dt,f,opt);
            
            /* set valid data flags */
            if (opt->mode>PMODE_DGPS) {
                if (f<nf) rtk->ssat[sat[i]-1].vsat[f]=rtk->ssat[sat[j]-1].vsat[f]=1; 
            }
            else {
                rtk->ssat[sat[i]-1].vsat[f-nf]=rtk->ssat[sat[j]-1].vsat[f-nf]=1;
            }
            trace(3,"sat=%3d-%3d %s%d v=%13.8f R=%8.6f %8.6f\n",sat[i],
                  sat[j],f<nf?"L":"P",f%nf+1,v[nv],Ri[nv],Rj[nv]);
            
			// add by Qijia Zhao
            // Restore the Double Difference information
			rtk->ddmeas[nv].code = f < nf ? 0 : 1;
			rtk->ddmeas[nv].f = f % nf;
			rtk->ddmeas[nv].refsat = sat[i];
			rtk->ddmeas[nv].urefsat = sat[j];
			rtk->ddmeas[nv].ref_index = i;
			rtk->ddmeas[nv].uref_index = j;
			rtk->ddmeas[nv].var = Ri[nv] + Rj[nv];
			if (f < nf) rtk->ddmeas[nv].ddbias = x[IB(sat[i], f, opt)] - x[IB(sat[j], f, opt)];
			rtk->ddmeas[nv].residual = v[nv];
			// end
			vflg[nv++]=(sat[i]<<16)|(sat[j]<<8)|((f<nf?0:1)<<4)|(f%nf);
            nb[b]++;
        }
        b++;
    }
    /* end of system loop */

    // add by Qijia Zhao
    rtk->bl = bl;
    rtk->dt = dt;
    // end


    /* baseline length constraint for moving baseline */
    if (opt->mode==PMODE_MOVEB&&constbl(rtk,x,P,v,H,Ri,Rj,nv)) {
        vflg[nv++]=3<<4;
        nb[b++]++;
    }
    if (H) {trace(5,"H=\n"); tracemat(5,H,rtk->nx,nv,7,4);}
    
    /* DD measurement error covariance */
    ddcov(nb,b,Ri,Rj,nv,R);

	
    
    free(Ri); free(Rj); free(im);
    free(tropu); free(tropr); free(dtdxu); free(dtdxr);
    
    return nv;
}
/* time-interpolation of residuals (for post-processing) ---------------------*/
static double intpres(gtime_t time, const obsd_t *obs, int n, const nav_t *nav,
                      rtk_t *rtk, double *y)
{
    static obsd_t obsb[MAXOBS];
    static double yb[MAXOBS*NFREQ*2],rs[MAXOBS*6],dts[MAXOBS*2],var[MAXOBS];
    static double e[MAXOBS*3],azel[MAXOBS*2],freq[MAXOBS*NFREQ];
    static int nb=0,svh[MAXOBS*2];
    prcopt_t *opt=&rtk->opt;
    double tt=timediff(time,obs[0].time),ttb,*p,*q;
    int i,j,k,nf=NF(opt);
    
    trace(3,"intpres : n=%d tt=%.1f\n",n,tt);
    
    if (nb==0||fabs(tt)<DTTOL) {
        nb=n; for (i=0;i<n;i++) obsb[i]=obs[i];
        return tt;
    }
    ttb=timediff(time,obsb[0].time);
    if (fabs(ttb)>opt->maxtdiff*2.0||ttb==tt) return tt;
    
    satposs(time,obsb,nb,nav,opt->sateph,rs,dts,var,svh);
    
    if (!zdres(1,obsb,nb,rs,dts,var,svh,nav,rtk->rb,opt,1,yb,e,azel,freq)) {
        return tt;
    }
    for (i=0;i<n;i++) {
        for (j=0;j<nb;j++) if (obsb[j].sat==obs[i].sat) break;
        if (j>=nb) continue;
        for (k=0,p=y+i*nf*2,q=yb+j*nf*2;k<nf*2;k++,p++,q++) {
            if (*p==0.0||*q==0.0) *p=0.0; else *p=(ttb*(*p)-tt*(*q))/(ttb-tt);
        }
    }
    return fabs(ttb)>fabs(tt)?ttb:tt;
}
/* index for SD to DD transformation matrix D --------------------------------*/
static int ddidx(rtk_t *rtk, int *ix)
{
    int i,j,k,m,f,nb=0,na=rtk->na,nf=NF(&rtk->opt),nofix;
    
    trace(3,"ddidx   :\n");
    
    for (i=0;i<MAXSAT;i++) for (j=0;j<NFREQ;j++) {
        rtk->ssat[i].fix[j]=0;
    }
    for (m=0;m<6;m++) { /* m=0:GPS/SBS,1:GLO,2:GAL,3:BDS,4:QZS,5:IRN */ 
        
        nofix=(m==1&&rtk->opt.glomodear==0)||(m==3&&rtk->opt.bdsmodear==0); 
        
        for (f=0,k=na;f<nf;f++,k+=MAXSAT) { 
            for (i=k;i<k+MAXSAT;i++) { 
                if (rtk->x[i]==0.0||!test_sys(rtk->ssat[i-k].sys,m)||
                    !rtk->ssat[i-k].vsat[f]||!rtk->ssat[i-k].half[f]) {
                    continue;
                }
                if (rtk->ssat[i-k].lock[f]>0&&!(rtk->ssat[i-k].slip[f]&2)&&
                    rtk->ssat[i-k].azel[1]>=rtk->opt.elmaskar&&!nofix) {
					rtk->ssat[i-k].fix[f]=2; /* fix */
                    break;
                }
                else rtk->ssat[i-k].fix[f]=1; /* float */
            }
            for (j=k;j<k+MAXSAT;j++) {
                if (i==j||rtk->x[j]==0.0||!test_sys(rtk->ssat[j-k].sys,m)||
                    !rtk->ssat[j-k].vsat[f]) {
                    continue;
                }
                if (rtk->ssat[j-k].lock[f]>0&&!(rtk->ssat[j-k].slip[f]&2)&&
                    rtk->ssat[i-k].vsat[f]&&
                    rtk->ssat[j-k].azel[1]>=rtk->opt.elmaskar&&!nofix) {
                    ix[nb*2  ]=i; /* state index of ref bias */
                    ix[nb*2+1]=j; /* state index of target bias */
                    nb++;
                    rtk->ssat[j-k].fix[f]=2; /* fix */
                }
                else rtk->ssat[j-k].fix[f]=1; /* float */
            }
        }
    }
    return nb;
}


/* restore SD (single-differenced) ambiguity ---------------------------------*/
static void restamb(rtk_t *rtk, const double *bias, int nb, double *xa)
{
    int i,n,m,f,index[MAXSAT],nv=0,nf=NF(&rtk->opt);
    
    trace(3,"restamb :\n");
    
    for (i=0;i<rtk->nx;i++) xa[i]=rtk->x [i];
    for (i=0;i<rtk->na;i++) xa[i]=rtk->xa[i];
    
    for (m=0;m<5;m++) for (f=0;f<nf;f++) {
        
        for (n=i=0;i<MAXSAT;i++) {
            if (!test_sys(rtk->ssat[i].sys,m)||rtk->ssat[i].fix[f]!=2) {
                continue;
            }
            index[n++]=IB(i+1,f,&rtk->opt); 
        }
        if (n<2) continue;
        
        xa[index[0]]=rtk->x[index[0]]; 
        
        for (i=1;i<n;i++) {
            xa[index[i]]=xa[index[0]]-bias[nv++]; 
        }
    }
}

/* hold integer ambiguity ----------------------------------------------------*/
static void holdamb(rtk_t *rtk, const double *xa)
{
    double *v,*H,*R;
    int i,n,m,f,info,index[MAXSAT],nb=rtk->nx-rtk->na,nv=0,nf=NF(&rtk->opt);
    
    trace(3,"holdamb :\n");
    
    v=mat(nb,1); H=zeros(nb,rtk->nx);
    
    for (m=0;m<5;m++) for (f=0;f<nf;f++) {
        
        for (n=i=0;i<MAXSAT;i++) {
            if (!test_sys(rtk->ssat[i].sys,m)||rtk->ssat[i].fix[f]!=2||
                rtk->ssat[i].azel[1]<rtk->opt.elmaskhold) {
                continue;
            }
            index[n++]=IB(i+1,f,&rtk->opt);
            rtk->ssat[i].fix[f]=3; /* hold */
        }
        /* constraint to fixed ambiguity */
        for (i=1;i<n;i++) {
            v[nv]=(xa[index[0]]-xa[index[i]])-(rtk->x[index[0]]-rtk->x[index[i]]);
            
            H[index[0]+nv*rtk->nx]= 1.0;
            H[index[i]+nv*rtk->nx]=-1.0;
            nv++;
        }
    }
    if (nv>0) {
        R=zeros(nv,nv);
        for (i=0;i<nv;i++) R[i+i*nv]=VAR_HOLDAMB;
        
        /* update states with constraints */
        if ((info=filter(rtk->x,rtk->P,H,v,R,rtk->nx,nv))) {
            errmsg(rtk,"filter error (info=%d)\n",info);
        }
        free(R);
    }
    free(v); free(H);
}
/* resolve integer ambiguity by LAMBDA ---------------------------------------*/
static int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa)
{
    prcopt_t *opt=&rtk->opt;
    int i,j,nb,info,nx=rtk->nx,na=rtk->na;
    double *DP,*y,*b,*db,*Qb,*Qab,*QQ,s[2];
    int *ix;

    trace(3,"resamb_LAMBDA : nx=%d\n",nx);
    
    rtk->sol.ratio=0.0;
    
    if (rtk->opt.mode<=PMODE_DGPS||rtk->opt.modear==ARMODE_OFF||
        rtk->opt.thresar[0]<1.0) {
        return 0;
    }
    /* index of SD to DD transformation matrix D */
    ix=imat(nx,2); 
    if ((nb=ddidx(rtk,ix))<=0) { 
        errmsg(rtk,"no valid double-difference\n");
        free(ix);
        return 0;
    }

    y=mat(nb,1); DP=mat(nb,nx-na); b=mat(nb,2); db=mat(nb,1); Qb=mat(nb,nb);
    Qab=mat(na,nb); QQ=mat(na,nb);
    

    /* y=D*xc, Qb=D*Qc*D', Qab=Qac*D' */
    for (i=0;i<nb;i++) {
        y[i]=rtk->x[ix[i*2]]-rtk->x[ix[i*2+1]]; 
    }
    for (j=0;j<nx-na;j++) for (i=0;i<nb;i++) { 
        DP[i+j*nb]=rtk->P[ix[i*2]+(na+j)*nx]-rtk->P[ix[i*2+1]+(na+j)*nx];
    }
    for (j=0;j<nb;j++) for (i=0;i<nb;i++) {
        Qb[i+j*nb]=DP[i+(ix[j*2]-na)*nb]-DP[i+(ix[j*2+1]-na)*nb];
    }
    for (j=0;j<nb;j++) for (i=0;i<na;i++) {
        Qab[i+j*na]=rtk->P[i+ix[j*2]*nx]-rtk->P[i+ix[j*2+1]*nx];
    }

	ddfix_init(rtk);
    /* LAMBDA/MLAMBDA ILS (integer least-square) estimation */
    if (!(info=lambda(nb,2,y,Qb,b,s))) { 

		// add by Qijia Zhao
        // Restore the fixed DD ambiguities information
		rtk->nb = nb; 
		for (i = 0; i < nb; i++) {
			rtk->ddfix[i].code = 0;
			rtk->ddfix[i].f = floor((ix[2 * i] - NR(opt) + 1) / MAXSAT);
			rtk->ddfix[i].refsat = SAT(ix[2 * i], rtk->ddfix[i].f, opt);
			rtk->ddfix[i].urefsat = SAT(ix[2 * i + 1], rtk->ddfix[i].f, opt);
			rtk->ddfix[i].ddbias = b[i];
		}
		// end

        trace(2,"N(1)="); tracemat(2,b   ,1,nb,10,3);
        trace(2,"N(2)="); tracemat(2,b+nb,1,nb,10,3);
        
        rtk->sol.ratio=s[0]>0?(float)(s[1]/s[0]):0.0f;
        if (rtk->sol.ratio>999.9) rtk->sol.ratio=999.9f;
        
        /* validation by popular ratio-test */
        if (s[0]<=0.0||s[1]/s[0]>=opt->thresar[0]) {
            
            /* transform float to fixed solution (xa=xa-Qab*Qb\(b0-b)) */
            for (i=0;i<na;i++) {
                rtk->xa[i]=rtk->x[i];
                for (j=0;j<na;j++) rtk->Pa[i+j*na]=rtk->P[i+j*nx];
            }
            for (i=0;i<nb;i++) {
                bias[i]=b[i]; 
                y[i]-=b[i]; 
            }
            if (!matinv(Qb,nb)) { 
                matmul("NN",nb,1,nb, 1.0,Qb ,y,0.0,db); // Qb^(-1)*(float-fix)
                matmul("NN",na,1,nb,-1.0,Qab,db,1.0,rtk->xa); // xa_fix = xa_float - Qab*Qb^(-1)*(float-fix)
                
                /* covariance of fixed solution (Qa=Qa-Qab*Qb^-1*Qab') */
                matmul("NN",na,nb,nb, 1.0,Qab,Qb ,0.0,QQ); // Qab*Qb^-1
                matmul("NT",na,na,nb,-1.0,QQ ,Qab,1.0,rtk->Pa); // Qa=Qa-Qab*Qb^-1*Qab'
                
                trace(3,"resamb : validation ok (nb=%d ratio=%.2f s=%.2f/%.2f)\n",
                      nb,s[0]==0.0?0.0:s[1]/s[0],s[0],s[1]);
                
                /* restore SD ambiguity */
                restamb(rtk,bias,nb,xa);
            }
            else nb=0;
        }
        else { /* validation failed */
            errmsg(rtk,"ambiguity validation failed (nb=%d ratio=%.2f s=%.2f/%.2f)\n",
                   nb,s[1]/s[0],s[0],s[1]);
            nb=0;
        }
    }
    else {
        errmsg(rtk,"lambda error (info=%d)\n",info);
        nb=0;
    }
    free(ix);
    free(y); free(DP); free(b); free(db); free(Qb); free(Qab); free(QQ);
    
    return nb; /* number of ambiguities */
}
/* validation of solution ----------------------------------------------------*/
static int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg,
                  int nv, double thres)
{
    double fact=thres*thres;
    int i,stat=1,sat1,sat2,type,freq;
    char *stype;
    
    trace(3,"valpos  : nv=%d thres=%.1f\n",nv,thres);
    
    /* post-fit residual test */
    for (i=0;i<nv;i++) {
        if (v[i]*v[i]<=fact*R[i+i*nv]) continue; 
        sat1=(vflg[i]>>16)&0xFF;
        sat2=(vflg[i]>> 8)&0xFF;
        type=(vflg[i]>> 4)&0xF;
        freq=vflg[i]&0xF;
        //stype=type==0? (char*)"L":(type==1? (char*)"L": (char*)"C");
		stype = type == 0 ? (char*)"L" : (char*)"P";
        errmsg(rtk,"large residual (sat=%2d-%2d %s%d v=%6.3f sig=%.3f)\n",
              sat1,sat2,stype,freq+1,v[i],SQRT(R[i+i*nv]));
    }
    return stat;
}
/* relative positioning ------------------------------------------------------*/
static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
                  const nav_t *nav)
{
    prcopt_t *opt=&rtk->opt;
    gtime_t time=obs[0].time;
    double *rs,*dts,*var,*y,*e,*azel,*freq,*v,*H,*R,*xp,*Pp,*xa,*bias,dt;
	// rcv:1-rover,2-base
    int i,j,f,n=nu+nr,ns,ny,nv = 0,sat[MAXSAT],iu[MAXSAT],ir[MAXSAT],niter; 
    int info,vflg[MAXOBS*NFREQ*2+1],svh[MAXOBS*2];
    int stat=rtk->opt.mode<=PMODE_DGPS?SOLQ_DGPS:SOLQ_FLOAT;
    int nf=opt->ionoopt==IONOOPT_IFLC?1:opt->nf;
	rtk->nb = 0;
    trace(3,"relpos  : nx=%d nu=%d nr=%d\n",rtk->nx,nu,nr);
    
    dt=timediff(time,obs[nu].time);
    
    rs=zeros(6,n); dts=zeros(2,n); var=zeros(1,n); y=zeros(nf*2,n); e=zeros(3,n);
    // y
    azel=zeros(2,n); freq=zeros(nf,n);
    
    for (i=0;i<MAXSAT;i++) {
        rtk->ssat[i].sys=satsys(i+1,NULL);
        for (j=0;j<NFREQ;j++) rtk->ssat[i].vsat[j]=0;
        for (j=1;j<NFREQ;j++) rtk->ssat[i].snr [j]=0;
    }
    /* satellite positions/clocks */
    satposs(time,obs,n,nav,opt->sateph,rs,dts,var,svh);
    
    /* UD (undifferenced) residuals for base station */
    if (!zdres(1,obs+nu,nr,rs+nu*6,dts+nu*2,var+nu,svh+nu,nav,rtk->rb,opt,1,
               y+nu*nf*2,e+nu*3,azel+nu*2,freq+nu*nf)) {
        errmsg(rtk,"initial base station position error\n");
        
        free(rs); free(dts); free(var); free(y); free(e); free(azel);
        free(freq);
        return 0;
    }
    /* time-interpolation of residuals (for post-processing) */
    if (opt->intpref) {
        dt=intpres(time,obs+nu,nr,nav,rtk,y+nu*nf*2);
    }
    /* select common satellites between rover and base-station */
    if ((ns=selsat(obs,azel,nu,nr,opt,sat,iu,ir))<=0) {
        errmsg(rtk,"no common satellite\n");
        
        free(rs); free(dts); free(var); free(y); free(e); free(azel);
        free(freq);
        return 0;
    }

    /* temporal update of states */
    udstate(rtk,obs,sat,iu,ir,ns,nav);
    
    trace(4,"x(0)="); tracemat(4,rtk->x,1,NR(opt),13,4);
    
    xp=mat(rtk->nx,1); Pp=zeros(rtk->nx,rtk->nx); xa=mat(rtk->nx,1); 
    matcpy(xp,rtk->x,rtk->nx,1); 
    
    ny=ns*nf*2+2; 
    v=mat(ny,1); H=zeros(rtk->nx,ny); R=mat(ny,ny); bias=mat(rtk->nx,1);
    
    /* add 2 iterations for baseline-constraint moving-base */
    niter=opt->niter+(opt->mode==PMODE_MOVEB&&opt->baseline[0]>0.0?2:0);
    
    for (i=0;i<niter;i++) {
        /* UD (undifferenced) residuals for rover */
        if (!zdres(0,obs,nu,rs,dts,var,svh,nav,xp,opt,0,y,e,azel,freq)) {
            errmsg(rtk,"rover initial position error\n");
            stat=SOLQ_NONE;
            break;
        }

        /* DD (double-differenced) residuals and partial derivatives */
        if ((nv=ddres(rtk,nav,dt,xp,Pp,sat,y,e,azel,freq,iu,ir,ns,v,H,R,
                      vflg))<1) {
            errmsg(rtk,"no double-differenced residual\n");
            stat=SOLQ_NONE;
            break;
        }

        /* Kalman filter measurement update */
        matcpy(Pp,rtk->P,rtk->nx,rtk->nx);
        if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) { 
            errmsg(rtk,"filter error (info=%d)\n",info);
            stat=SOLQ_NONE;
            break;
        }
        trace(4,"x(%d)=",i+1); tracemat(4,xp,1,NR(opt),13,4);
    }

    if (stat!=SOLQ_NONE&&zdres(0,obs,nu,rs,dts,var,svh,nav,xp,opt,0,y,e,azel,
                               freq)) {
        
        /* post-fit residuals for float solution */
        nv=ddres(rtk,nav,dt,xp,Pp,sat,y,e,azel,freq,iu,ir,ns,v,NULL,R,vflg); 
        
        /* validation of float solution */
        if (valpos(rtk,v,R,vflg,nv,4.0)) {
            
            /* update state and covariance matrix */
            matcpy(rtk->x,xp,rtk->nx,1); 
            matcpy(rtk->P,Pp,rtk->nx,rtk->nx);
            
            /* update ambiguity control struct */
            rtk->sol.ns=0;
            for (i=0;i<ns;i++) for (f=0;f<nf;f++) {
                if (!rtk->ssat[sat[i]-1].vsat[f]) continue;
                rtk->ssat[sat[i]-1].lock[f]++; 
                rtk->ssat[sat[i]-1].outc[f]=0;
                if (f==0) rtk->sol.ns++; /* valid satellite count by L1 */
            }
            /* lack of valid satellites */
            if (rtk->sol.ns<4) stat=SOLQ_NONE;
        }
        else stat=SOLQ_NONE;
    }

    /* resolve integer ambiguity by LAMBDA */
    if (stat!=SOLQ_NONE&&resamb_LAMBDA(rtk,bias,xa)>1) {
        
        if (zdres(0,obs,nu,rs,dts,var,svh,nav,xa,opt,0,y,e,azel,freq)) {
            
            /* post-fit reisiduals for fixed solution */
            nv=ddres(rtk,nav,dt,xa,NULL,sat,y,e,azel,freq,iu,ir,ns,v,NULL,R,
                     vflg);
            
            /* validation of fixed solution */
            if (valpos(rtk,v,R,vflg,nv,4.0)) {
                
                /* hold integer ambiguity */
                if (++rtk->nfix>=rtk->opt.minfix&&
                    rtk->opt.modear==ARMODE_FIXHOLD) {
                    holdamb(rtk,xa);
                }
				// add by Qijia Zhao
				memcpy(rtk->xfix, xa, sizeof(double) * rtk->nx);
				// end
                stat=SOLQ_FIX;
            }
        }
    }

	// add by Qijia Zhao
    rtk->ns = ns;
	rtk->nv = nv;
    rtk->nn = n;
    rtk->nr = nr;
    rtk->nu = nu;
	rtk->dt = dt;

	memcpy(rtk->iu, iu, sizeof(int) * rtk->ns);
	memcpy(rtk->ir, ir, sizeof(int)* rtk->ns);
    memcpy(rtk->nsat, sat, sizeof(int) * ns);
	// end
	

    /* save solution status */
    if (stat==SOLQ_FIX) {
        for (i=0;i<3;i++) {
            rtk->sol.rr[i]=rtk->xa[i];
            rtk->sol.qr[i]=(float)rtk->Pa[i+i*rtk->na];
        }
        rtk->sol.qr[3]=(float)rtk->Pa[1];
        rtk->sol.qr[4]=(float)rtk->Pa[1+2*rtk->na];
        rtk->sol.qr[5]=(float)rtk->Pa[2];
        
        if (rtk->opt.dynamics) { /* velocity and covariance */
            for (i=3;i<6;i++) {
                rtk->sol.rr[i]=rtk->xa[i];
                rtk->sol.qv[i-3]=(float)rtk->Pa[i+i*rtk->na];
            }
            rtk->sol.qv[3]=(float)rtk->Pa[4+3*rtk->na];
            rtk->sol.qv[4]=(float)rtk->Pa[5+4*rtk->na];
            rtk->sol.qv[5]=(float)rtk->Pa[5+3*rtk->na];
        }
    }
    else {
        for (i=0;i<3;i++) {
            rtk->sol.rr[i]=rtk->x[i];
            rtk->sol.qr[i]=(float)rtk->P[i+i*rtk->nx];
        }
        rtk->sol.qr[3]=(float)rtk->P[1];
        rtk->sol.qr[4]=(float)rtk->P[1+2*rtk->nx];
        rtk->sol.qr[5]=(float)rtk->P[2];
        
        if (rtk->opt.dynamics) { /* velocity and covariance */
            for (i=3;i<6;i++) {
                rtk->sol.rr[i]=rtk->x[i];
                rtk->sol.qv[i-3]=(float)rtk->P[i+i*rtk->nx];
            }
            rtk->sol.qv[3]=(float)rtk->P[4+3*rtk->nx];
            rtk->sol.qv[4]=(float)rtk->P[5+4*rtk->nx];
            rtk->sol.qv[5]=(float)rtk->P[5+3*rtk->nx];
        }
        rtk->nfix=0; 
    }
    for (i=0;i<n;i++) for (j=0;j<nf;j++) {
        if (obs[i].L[j]==0.0) continue;
        rtk->ssat[obs[i].sat-1].pt[obs[i].rcv-1][j]=obs[i].time;
        rtk->ssat[obs[i].sat-1].ph[obs[i].rcv-1][j]=obs[i].L[j];
    }
    for (i=0;i<ns;i++) for (j=0;j<nf;j++) {
        
        /* output snr of rover receiver */
        rtk->ssat[sat[i]-1].snr[j]=obs[iu[i]].SNR[j];
    }
    for (i=0;i<MAXSAT;i++) for (j=0;j<nf;j++) {
        if (rtk->ssat[i].fix[j]==2&&stat!=SOLQ_FIX) rtk->ssat[i].fix[j]=1; 
        if (rtk->ssat[i].slip[j]&1) rtk->ssat[i].slipc[j]++;
    }

    if (stat != SOLQ_NONE) rtk->sol.stat = stat;


	// add by Qijia Zhao
    if (stat != SOLQ_NONE) {
        if (save_node_data(rtk, stat, opt, nav, obs, n, rs, dts, var, svh, y, R, azel, e)) {
			RANSAC_Doppler_shift_quality_control(node_list.back());
			VFG_optimization();
            PFG_optimization();
            if (node_list.back()->stat == SOLQ_FIX) {
                have_first_fix = true;
            }
        }
        
        // Output the results
        outputdata(node_list.back(), rtk);

        // Sliding window
        if (node_list.size() >= window_size) {
			VFG_marginalization();
            PFG_marginalization();
            sliding_window();
        }
        
    }
    
	epoch_now++;
    // end


    free(rs); free(dts); free(var); free(y); free(e); free(azel); free(freq);
    free(xp); free(Pp);  free(xa);  free(v); free(H); free(R); free(bias);
    
    return stat!=SOLQ_NONE;
}

// add by Qijia Zhao
/** 
 * @brief Saves the information into the node.
 * 
 * 
 * @param rtk Pointer to the RTK control struct.
 * @param stat The solution status (e.g., SOLQ_NONE, SOLQ_FIX).
 * @param popt The processing options.
 * @param nav Pointer to the navigation data.
 * @param obs Array of observation data.
 * @param nobs Number of observations.
 * @param rs Satellite position data.
 * @param dts Satellite clock biases.
 * @param var Observation variances.
 * @param svh Satellite health flags.
 * @param y The measurement vector.
 * @param R The measurement covariance matrix.
 * @param azel Azimuth and elevation angles for the satellites.
 * @param e Satellite directional vector.
 * 
 * @return 1 on success, 0 on failure.
 */
int save_node_data(rtk_t* rtk, const int stat, const prcopt_t* popt, const nav_t* nav, const obsd_t* obs, const int nobs,
    double* rs, double* dts, double* var, int* svh, double* y, double* R, double* azel, double* e) {

    if (stat == SOLQ_NONE) {
        return 0; // No solution, return 0
    }

    int i, j;

    // Create a new node
    node_info *node = new node_info;
    node->nepoch = epoch_now;

    // Initialize ddmeas (DD information)
    node->ddmeas = new ddinfo[rtk->nv];
    set<int> fixflag;
    int flag = 0;

    if (rtk->nb > 0) {
        node->ddfix = new ddinfo[rtk->nb];
        for (int j = 0; j < rtk->nb; j++) {
            flag = (rtk->ddfix[j].refsat << 8) | rtk->ddfix[j].f;
            fixflag.insert(flag);
            flag = (rtk->ddfix[j].urefsat << 8) | rtk->ddfix[j].f;
            fixflag.insert(flag);
        }
    }

    // Save the information from RTK
    rtkinformation(node, rtk, fixflag);

    int nf = popt->ionoopt == IONOOPT_IFLC ? 1 : popt->nf;
    node->rs = mat(6, node->nn); matcpy(node->rs, rs, 6, node->nn);
    node->dts = mat(2, node->nn); matcpy(node->dts, dts, 2, node->nn);
    node->var = mat(1, node->nn); matcpy(node->var, var, 1, node->nn);
    node->R = mat(node->nv, node->nv); matcpy(node->R, R, node->nv, node->nv);
    node->azel = mat(2, node->nn); matcpy(node->azel, azel, 2, node->nn);
    node->e = mat(3, node->nn); matcpy(node->e, e, 3, node->nn);
    memcpy(node->svh, svh, sizeof(int) * node->nn);
    node->d_base = mat(node->nv, 1);

    // Compute the SD measurements from the base station
    for (i = 0; i < node->nv; i++) {
        if (node->ddmeas[i].code)
            node->d_base[i] = y[node->ddmeas[i].f + nf + rtk->ir[node->ddmeas[i].ref_index] * nf * 2] - y[node->ddmeas[i].f + nf + rtk->ir[node->ddmeas[i].uref_index] * nf * 2];
        else
            node->d_base[i] = y[node->ddmeas[i].f + rtk->ir[node->ddmeas[i].ref_index] * nf * 2] - y[node->ddmeas[i].f + rtk->ir[node->ddmeas[i].uref_index] * nf * 2];
    }

    node->xf = mat(rtk->na, 1);
    node->Pf = mat(rtk->na, rtk->na);
    node->xa = mat(rtk->na, 1);
    node->Pa = mat(rtk->na, rtk->na);

    // Restore the initialized state vector and covariance matrix
    if (stat != SOLQ_FIX) {
        for (i = 0; i < rtk->na; i++) {
            node->xf[i] = rtk->x[i];
            for (j = 0; j < rtk->na; j++)
                node->Pf[i + j * rtk->na] = rtk->P[i + j * rtk->nx];
        }
    } else {
        for (i = 0; i < rtk->na; i++) {
            node->xf[i] = rtk->xa[i];
            node->xa[i] = rtk->xa[i];
            for (j = 0; j < rtk->na; j++) {
                node->Pf[i + j * rtk->na] = rtk->Pa[i + j * rtk->na];
                node->Pa[i + j * rtk->na] = rtk->Pa[i + j * rtk->na];
            }
        }
    }

    // Restore the initialized velocity
    for (i = 0; i < 3; i++) {
        node->xf[3 + i] = rtk->sol.rr[3 + i];
    }

    // Save observation data
    node->nobs = nobs;
    memcpy(&(node->obs), obs, sizeof(obsd_t) * nobs);

    // Remove SD ambiguity with no nodes connected
    for (auto it = sdambi_info_list.begin(); it != sdambi_info_list.end();) {
        if ((*it)->num <= 0) {
            int index = it - sdambi_info_list.begin();
            delete sdambi_info_list[index];
            sdambi_info_list[index] = nullptr;
            index_adjust(index);
            it = sdambi_info_list.erase(it);
        } else {
            ++it;
        }
    }

    // Manage SD ambiguities and model them in GSSM
    map<int, int> snmap;
    for (i = 0; i < node->nv; i++) {
        if (node->ddmeas[i].code) {
            continue;
        }

        int ref_sdvflg = (node->ddmeas[i].refsat << 8) | node->ddmeas[i].f;
        int ref_fixflag = 1;
        if (fixflag.find(ref_sdvflg) != fixflag.end()) {
            ref_fixflag = 2;
            if (node->stat == SOLQ_FIX) {
                ref_fixflag = 3;
            }
        }
        sdambi_gssm(&snmap, node, rtk, ref_sdvflg, ref_fixflag);

        int uref_sdvflg = (node->ddmeas[i].urefsat << 8) | node->ddmeas[i].f;
        int uref_fixflag = 1;
        if (fixflag.find(uref_sdvflg) != fixflag.end()) {
            uref_fixflag = 2;
            if (node->stat == SOLQ_FIX) {
                uref_fixflag = 3;
            }
        }
        sdambi_gssm(&snmap, node, rtk, uref_sdvflg, uref_fixflag);
    }

    // Update SD ambiguity indices in ddmeas
    for (i = 0; i < node->nv; i++) {
        if (node->ddmeas[i].code) {
            continue;
        }

        // Reference SD ambiguity index
        int ref_sdflg = (node->ddmeas[i].refsat << 8) | (node->ddmeas[i].f);
        map<int, int>::iterator refit = snmap.find(ref_sdflg);
        node->ddmeas[i].ref_sdambi_index = refit->second;

        // Unreference SD ambiguity index
        int uref_sdflg = (node->ddmeas[i].urefsat << 8) | (node->ddmeas[i].f);
        map<int, int>::iterator urefit = snmap.find(uref_sdflg);
        node->ddmeas[i].uref_sdambi_index = urefit->second;
    }

    // Update SD ambiguity indices in ddfix
    for (i = 0; i < node->nb; i++) {
        int ref_sdflg = (node->ddfix[i].refsat << 8) | (node->ddfix[i].f);
        map<int, int>::iterator refit = snmap.find(ref_sdflg);
        node->ddfix[i].ref_sdambi_index = refit->second;

        int uref_sdflg = (node->ddfix[i].urefsat << 8) | (node->ddfix[i].f);
        map<int, int>::iterator urefit = snmap.find(uref_sdflg);
        node->ddfix[i].uref_sdambi_index = urefit->second;
    }

    // Add the new node to the node list
    node_list.push_back(node);

    return 1; // Success
}


/** 
 * @brief Saves RTK solution data into node information.
 * 
 * 
 * @param node The node information.
 * @param rtk Pointer to the RTK control struct.
 * @param fixflag Set of fixed ambiguities flags.
 */
void rtkinformation(node_info* node, rtk_t* rtk, std::set<int> &fixflag) {
    int nvv = 0, nbb = 0;

    // Save solution time, status, and relevant parameters
    node->time = rtk->sol.time;
    node->stat = rtk->sol.stat;
    node->ratio = rtk->sol.ratio;
    node->bl = rtk->bl;
    node->nn = rtk->nn;
    node->nu = rtk->nu;
    node->nr = rtk->nr;
    node->ns = rtk->ns;
    node->nx = rtk->nx;
    node->na = rtk->na;
    node->nb = rtk->nb;
    node->tt = rtk->tt;
    node->dt = rtk->dt;

    memcpy(node->iu, rtk->iu, sizeof(int) * MAXSAT);
    memcpy(node->ir, rtk->ir, sizeof(int) * MAXSAT);
    memcpy(node->nsat, rtk->nsat, sizeof(int) * MAXSAT);
    memcpy(node->ssat, rtk->ssat, sizeof(ssat_t) * MAXSAT);

    // Validate and store double-differenced measurements
    for (int i = 0; i < rtk->nv; i++) {
        if (rtk->ddmeas[i].isValid) {
            if (rtk->ddmeas[i].code && fabs(rtk->ddmeas[i].residual) >= 3.0) {
                rtk->ddmeas[i].isValid = false;
                for (int j = 0; j < rtk->nv; j++) {
                    if (!rtk->ddmeas[j].code && rtk->ddmeas[j].refsat == rtk->ddmeas[i].refsat &&
                        rtk->ddmeas[j].urefsat == rtk->ddmeas[i].urefsat && rtk->ddmeas[j].f == rtk->ddmeas[i].f) {
                        rtk->ddmeas[j].isValid = false;
                    }
                }
            } else if (!rtk->ddmeas[i].code && fabs(rtk->ddmeas[i].residual) >= 0.08) {
                rtk->ddmeas[i].isPrior = false;
            }
        }
    }

    // Add valid measurements to node
    for (int i = 0; i < rtk->nv; i++) {
        if (fixflag.size() > 0 && (fixflag.find((rtk->ddmeas[i].refsat << 8) | rtk->ddmeas[i].f) != fixflag.end() ||
            fixflag.find((rtk->ddmeas[i].urefsat << 8) | rtk->ddmeas[i].f) != fixflag.end())) {
            rtk->ddmeas[i].isValid = true;
        }
        if (rtk->ddmeas[i].isValid) {
            node->ddmeas[nvv++] = rtk->ddmeas[i];
        }
    }
    node->nv = nvv;

    // Add valid fixed ambiguities to node
    if (rtk->nb > 0) {
        for (int i = 0; i < rtk->nb; i++) {
            if (rtk->ddfix[i].isValid) {
                node->ddfix[nbb++] = rtk->ddfix[i];
            }
        }
        node->nb = nbb;
    }
}



/** 
 * @brief Attach a new SD ambiguity to the list.
 * 
 * 
 * @param node The node information.
 * @param rtk Pointer to the RTK control struct.
 * @param sdflg The SD flag representing the satellite and frequency.
 * @param stat The status of the SD ambiguity.
 */
void attach_new_sdambi(node_info* node, rtk_t* rtk, int sdflg, int stat) {
    // Extract satellite index and frequency from the SD flag
    int sat = (sdflg >> 8) & 0xFF;
    int f = sdflg & 0xFF;
    
    // Get the index for the SD ambiguity
    int index = IB(sat, f, &option);
    
    // Get the SD bias from fixed or floating solution
    double sdbias = (stat > 1) ? rtk->xfix[index] : rtk->x[index];
    
    // Get the variance from the covariance matrix
    double var = rtk->P[index + rtk->nx * index];
    
    // Initialize SD ambiguity information
    init_sdambi_infor(rtk, sdbias, var, node->nepoch, sdflg, stat);
}


/** 
 * @brief Initialize a new SD ambiguity parameter.
 * 
 * 
 * @param rtk Pointer to the RTK control struct.
 * @param sdbias The value of the SD ambiguity
 * @param var The variance of the SD ambiguity
 * @param epoch_s The epoch from which the parameter is initialized
 * @param sdflg The SD flag representing the satellite and frequency.
 * @param stat The status of the SD ambiguity.
 */
void init_sdambi_infor(rtk_t *rtk, double sdbias, double var, int epoch_s, int sdflg, int stat) {
	sdambi_info *sdambi = new sdambi_info;
	sdambi->epoch_s = epoch_s;
	sdambi->epoch_e = -1;
	sdambi->Ismarg = false;
	sdambi->num = 1;
	sdambi->sdbias = sdbias;
	sdambi->var = var;
	sdambi->f = sdflg & 0xFF;
	sdambi->sat = (sdflg >> 8) & 0xFF;
	sdambi->sdflg = sdflg;
    sdambi->flag = stat;
	sdambi->bias_value.push_back(sdbias);
	sdambi->bias_var.push_back(var);
	sdambi->nepochs.push_back(epoch_now);
	sdambi_info_list.push_back(sdambi);
}


/** 
 * @brief Merge two SD ambiguity parameters.
 * 
 * 
 * @param rtk Pointer to the RTK control struct.
 * @param sdambi Pointer to the SD ambiguity information struct
 * @param stat The status of the SD ambiguity.
 */
void sdambi_merge(rtk_t *rtk, sdambi_info *sdambi, int stat) {
	int sat = sdambi->sat;
	int f = sdambi->f;
	int index = IB(sat, f, &(option));

    // Update the value and variance
    if (stat == 3) {
		trace(3, "sdbias_merge: new bias = %lf, new var = %lf\n", rtk->xfix[index], rtk->P[index + rtk->nx * index]);
		sdambi->sdbias = rtk->xfix[index];
		sdambi->var = rtk->P[index + rtk->nx * index];
		sdambi->flag = stat;
		trace(3, "sdbias_merge: sdbias fix!\n");
	}
    // Calculate the average
    else if (stat == 2) {
        trace(3, "sdbias_merge: new bias = %lf, new var = %lf\n", rtk->x[index], rtk->P[index + rtk->nx * index]);
		double sum_bias = sdambi->sdbias * sdambi->num + rtk->x[index];
		double sum_var = sdambi->var * sdambi->num + rtk->P[index + rtk->nx * index];
		sdambi->sdbias = sum_bias / double(sdambi->num + 1);
		sdambi->var = sum_var / double(sdambi->num + 1);
		sdambi->flag = stat;
		trace(3, "sdbias_merge: sdbias ready to fix!\n");
    }
	else {
		trace(3, "sdbias_merge: new bias = %lf, new var = %lf\n", rtk->x[index], rtk->P[index + rtk->nx * index]);
		double sum_bias = sdambi->sdbias * sdambi->num + rtk->x[index];
		double sum_var = sdambi->var * sdambi->num + rtk->P[index + rtk->nx * index];
		sdambi->sdbias = sum_bias / double(sdambi->num + 1);
		sdambi->var = sum_var / double(sdambi->num + 1);
		sdambi->flag = stat;
		trace(3, "sdbias_merge: sdbias float!\n");
	}

	sdambi->num++;
	sdambi->bias_value.push_back(rtk->x[index]);
	sdambi->bias_var.push_back(rtk->P[index + rtk->nx * index]);
	sdambi->nepochs.push_back(epoch_now);
	trace(4, "sdbias_merge: merge bias = %lf, merge var = %lf\n", sdambi->sdbias, sdambi->var);

	return;
}


/** 
 * @brief Model the ambiguities in GSSM.
 *
 *
 * @param snmap A map to track the SD ambiguity flags.
 * @param node The node information.
 * @param rtk Pointer to the RTK control struct.
 * @param sdflg SD ambiguity flag identifying the ambiguity.
 * @param stat The status of the SD ambiguity.
 */
void sdambi_gssm(map<int, int> *snmap, node_info *node, rtk_t *rtk, int sdflg, int stat) {
    // Check if the SD ambiguity flag already exists
	if (snmap->find(sdflg) != snmap->end()) {
		return;
	}

	bool isfind = true;
	deque<sdambi_info*>::iterator iter = sdambi_info_list.begin();

	// Search for existing SD ambiguity in the list
	while (true) {

		// Find SD ambiguity in the list matching the sdflg
		iter = find_if(iter, sdambi_info_list.end(), [sdflg](sdambi_info* obj) {
			return obj->sdflg == sdflg; });

		// If not found, create new SD ambiguity and insert it into the map
		if (iter == sdambi_info_list.end()) {
			isfind = false;
			attach_new_sdambi(node, rtk, sdflg, stat);
			snmap->insert(std::make_pair(sdflg, sdambi_info_list.size() - 1));
			break;
		}

		// If this epoch is prior to the ending epoch of the ambiguity
		if (((*iter)->epoch_e < 0) || ((*iter)->epoch_e >= epoch_now)) {
			break;
		}

		iter++;
	}

	// If SD ambiguity is found, update or merge it
	if (isfind) {
		int sat = (sdflg >> 8) & 0xFF;
		int f = sdflg & 0xFF;

        // Check if ambiguity should be updated or replaced
		if ((rtk->ssat[sat - 1].slip[f] & 1) || (rtk->ssat[sat - 1].rejc[f] > 2) ||
			(fabs(rtk->x[IB(sat, f, &option)] - (*iter)->sdbias) > 0.5)) {

			(*iter)->epoch_e = epoch_now - 1;
			attach_new_sdambi(node, rtk, sdflg, stat);
			snmap->insert(std::make_pair(sdflg, sdambi_info_list.size() - 1));
		}
		else {
			// Merge current ambiguity with the existing one
			sdambi_merge(rtk, (*iter), stat);
			snmap->insert(std::make_pair(sdflg, iter - sdambi_info_list.begin()));
		}
	}

	return;
}



/** 
 * @brief Initialize the ddmeas struct.
 * 
 * 
 * @param rtk Pointer to the RTK control struct.
 */
void ddmeas_init(rtk_t *rtk) {
	int i = 0;
	for (i = 0; i < MAXOBS * NFREQ * 2 + 1; i++) {
		rtk->ddmeas[i].refsat = 0; rtk->ddmeas[i].ref_sdambi_index = -1;
		rtk->ddmeas[i].urefsat = 0; rtk->ddmeas[i].uref_sdambi_index = -1;
		rtk->ddmeas[i].f = -1; rtk->ddmeas[i].code = -1;
		rtk->ddmeas[i].ref_index = -1; rtk->ddmeas[i].uref_index = -1;
		rtk->ddmeas[i].isValid = true; rtk->ddmeas[i].isPrior = true;
		rtk->ddmeas[i].var = 0.0;
		rtk->ddmeas[i].ddbias = 0.0;
        rtk->ddmeas[i].residual = 0.0;
	}
}


/** 
 * @brief Initialize the ddfix struct.
 * 
 * 
 * @param rtk Pointer to the RTK control struct.
 */
void ddfix_init(rtk_t *rtk) {
	int i = 0;
	rtk->nb = 0;
	for (i = 0; i < MAXOBS * NFREQ * 2 + 1; i++) {
		rtk->ddfix[i].refsat = 0; rtk->ddfix[i].ref_sdambi_index = -1;
		rtk->ddfix[i].urefsat = 0; rtk->ddfix[i].uref_sdambi_index = -1;
		rtk->ddfix[i].f = -1; rtk->ddfix[i].code = -1;
		rtk->ddfix[i].ref_index = -1; rtk->ddfix[i].uref_index = -1;
		rtk->ddfix[i].isValid = true; rtk->ddfix[i].isPrior = true;
		rtk->ddfix[i].var = 0.0;
		rtk->ddfix[i].ddbias = 0.0;
	}
}


/** 
 * @brief Free the memory of the node.
 * 
 * 
 * @param rtk Pointer to the RTK control struct.
 */
void node_free(node_info *node) {

	if (node->stat != SOLQ_NONE) {
		set<int> satindex;

		for (int i = 0; i < node->nv; i++) {
			if (node->ddmeas[i].code) continue;
			if (!node->ddmeas[i].isValid) continue;
			satindex.insert(node->ddmeas[i].ref_sdambi_index);
			satindex.insert(node->ddmeas[i].uref_sdambi_index);
		}

		for (set<int>::iterator it = satindex.begin(); it != satindex.end(); ++it) {
			sdambi_info_list.at(*it)->num -= 1;
			sdambi_info_list.at(*it)->bias_value.pop_front();
			sdambi_info_list.at(*it)->bias_var.pop_front();
			sdambi_info_list.at(*it)->nepochs.pop_front();
		}
	}

	node->nepoch = node->nobs = 0;

	if (node->nb > 0) delete[] node->ddfix;
	delete[] node->ddmeas;

    node->nn = node->nu = node->nr = node->nv = node->ns = node->nd = node->nx = node->na = node->nb = 0;

    free(node->xf); node->xf = NULL;
    free(node->Pf); node->Pf = NULL;
	free(node->xa); node->xa = NULL;
	free(node->Pa); node->Pa = NULL;
    free(node->rs); node->rs = NULL;
    free(node->dts); node->dts = NULL;
    free(node->var); node->var = NULL;
    free(node->R); node->R = NULL;
    free(node->azel); node->azel = NULL;
    free(node->d_base); node->d_base = NULL;
    free(node->e); node->e = NULL;

    delete[] node;
	node = NULL;
}


/** 
 * @brief Adjust the SD ambiguity indexes when sliding window.
 * 
 * 
 * @param index The index of the SD ambiguity sliding out of the window.
 */
void index_adjust(int index) {
    size_t j = 0;
    int k = 0;
    for (j = 0; j < node_list.size(); j++) {
        node_info* node = node_list.at(j);
		
        for (k = 0; k < node->nv; k++) {
			if (node->ddmeas[k].ref_sdambi_index > index) {
				node->ddmeas[k].ref_sdambi_index -= 1;
			}
			
			if (node->ddmeas[k].uref_sdambi_index > index) {
				node->ddmeas[k].uref_sdambi_index -= 1;
			}
        }

		if (node->nb > 0) {
			for (k = 0; k < node->nb; k++) {
				if (node->ddfix[k].ref_sdambi_index > index) {
					node->ddfix[k].ref_sdambi_index -= 1;
				}

				if (node->ddfix[k].uref_sdambi_index > index) {
					node->ddfix[k].uref_sdambi_index -= 1;
				}
			}
		}
    }
}

/** 
 * @brief Sliding window process.
 * 
 */
void sliding_window()
{
    // Free memory of the node
    int first_node_epoch = node_list.front()->nepoch;
	node_free(node_list.front());
	node_list.pop_front();

    // Delete the ambiguity without nodes connected to it
	for (auto it = sdambi_info_list.begin(); it != sdambi_info_list.end();) {
		if (*it == nullptr) {
			trace(3, "Check Nullptr!\n");
		}
		if ((((*it)->epoch_e >= 0) && (first_node_epoch - (*it)->epoch_e >= 0)) || ((*it)->num <= 0)) {
			int index = it - sdambi_info_list.begin();
			delete sdambi_info_list[index];
			sdambi_info_list[index] = nullptr;
			index_adjust(index);
			it = sdambi_info_list.erase(it);
		}
		else {
			++it;
		}
	}

    return;
}

// end



/* initialize RTK control ------------------------------------------------------
* initialize RTK control struct
* args   : rtk_t    *rtk    IO  TKk control/result struct
*          prcopt_t *opt    I   positioning options (see rtklib.h)
* return : none
*-----------------------------------------------------------------------------*/
extern void rtkinit(rtk_t *rtk, const prcopt_t *opt)
{
    sol_t sol0={{0}};
    ambc_t ambc0={{{0}}};
    ssat_t ssat0={0};
    int i;
    
    trace(3,"rtkinit :\n");
    
    rtk->sol=sol0;
    for (i=0;i<6;i++) rtk->rb[i]=0.0;
    rtk->nx=opt->mode<=PMODE_FIXED?NX(opt):pppnx(opt);
    rtk->na=opt->mode<=PMODE_FIXED?NR(opt):pppnx(opt);
    rtk->tt=0.0;
    rtk->x=zeros(rtk->nx,1);
    rtk->P=zeros(rtk->nx,rtk->nx);
    rtk->xa=zeros(rtk->na,1);
    rtk->Pa=zeros(rtk->na,rtk->na);
    rtk->nfix=rtk->neb=0;
    for (i=0;i<MAXSAT;i++) {
        rtk->ambc[i]=ambc0;
        rtk->ssat[i]=ssat0;
    }
    for (i=0;i<MAXERRMSG;i++) rtk->errbuf[i]=0;
    rtk->opt=*opt;

	// add by Qijia Zhao
	rtk->xfix = zeros(rtk->nx, 1);
	// end
}

/* free rtk control ------------------------------------------------------------
* free memory for rtk control struct
* args   : rtk_t    *rtk    IO  rtk control/result struct
* return : none
*-----------------------------------------------------------------------------*/
extern void rtkfree(rtk_t *rtk)
{
    trace(3,"rtkfree :\n");
    
    rtk->nx=rtk->na=0;
    free(rtk->x ); rtk->x =NULL;
    free(rtk->P ); rtk->P =NULL;
    free(rtk->xa); rtk->xa=NULL;
    free(rtk->Pa); rtk->Pa=NULL;
	// add by Qijia Zhao
	free(rtk->xfix); rtk->xfix = NULL;
	// end
}
/* precise positioning ---------------------------------------------------------
* input observation data and navigation message, compute rover position by 
* precise positioning
* args   : rtk_t *rtk       IO  RTK control/result struct
*            rtk->sol       IO  solution
*                .time      O   solution time
*                .rr[]      IO  rover position/velocity
*                               (I:fixed mode,O:single mode)
*                .dtr[0]    O   receiver clock bias (s)
*                .dtr[1-5]  O   receiver GLO/GAL/BDS/IRN/QZS-GPS time offset (s)
*                .Qr[]      O   rover position covarinace
*                .stat      O   solution status (SOLQ_???)
*                .ns        O   number of valid satellites
*                .age       O   age of differential (s)
*                .ratio     O   ratio factor for ambiguity validation
*            rtk->rb[]      IO  base station position/velocity
*                               (I:relative mode,O:moving-base mode)
*            rtk->nx        I   number of all states
*            rtk->na        I   number of integer states
*            rtk->ns        O   number of valid satellites in use
*            rtk->tt        O   time difference between current and previous (s)
*            rtk->x[]       IO  float states pre-filter and post-filter
*            rtk->P[]       IO  float covariance pre-filter and post-filter
*            rtk->xa[]      O   fixed states after AR
*            rtk->Pa[]      O   fixed covariance after AR
*            rtk->ssat[s]   IO  satellite {s+1} status
*                .sys       O   system (SYS_???)
*                .az   [r]  O   azimuth angle   (rad) (r=0:rover,1:base)
*                .el   [r]  O   elevation angle (rad) (r=0:rover,1:base)
*                .vs   [r]  O   data valid single     (r=0:rover,1:base)
*                .resp [f]  O   freq(f+1) pseudorange residual (m)
*                .resc [f]  O   freq(f+1) carrier-phase residual (m)
*                .vsat [f]  O   freq(f+1) data vaild (0:invalid,1:valid)
*                .fix  [f]  O   freq(f+1) ambiguity flag
*                               (0:nodata,1:float,2:fix,3:hold)
*                .slip [f]  O   freq(f+1) cycle slip flag
*                               (bit8-7:rcv1 LLI, bit6-5:rcv2 LLI,
*                                bit2:parity unknown, bit1:slip)
*                .lock [f]  IO  freq(f+1) carrier lock count
*                .outc [f]  IO  freq(f+1) carrier outage count
*                .slipc[f]  IO  freq(f+1) cycle slip count
*                .rejc [f]  IO  freq(f+1) data reject count
*                .gf        IO  geometry-free phase (L1-L2) (m)
*                .gf2       IO  geometry-free phase (L1-L5) (m)
*            rtk->nfix      IO  number of continuous fixes of ambiguity
*            rtk->neb       IO  bytes of error message buffer
*            rtk->errbuf    IO  error message buffer
*            rtk->tstr      O   time string for debug
*            rtk->opt       I   processing options
*          obsd_t *obs      I   observation data for an epoch
*                               obs[i].rcv=1:rover,2:reference
*                               sorted by receiver and satellte
*          int    n         I   number of observation data
*          nav_t  *nav      I   navigation messages
* return : status (0:no solution,1:valid solution)
* notes  : before calling function, base station position rtk->sol.rb[] should
*          be properly set for relative mode except for moving-baseline
*-----------------------------------------------------------------------------*/
extern int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
{
    prcopt_t *opt=&rtk->opt;
    sol_t solb={{0}};
    gtime_t time;
    int i=0,nu=0,nr=0;
    char msg[128]="";
    
    trace(3,"rtkpos  : time=%s n=%d\n",time_str(obs[0].time,3),n);
    trace(4,"obs=\n"); traceobs(4,obs,n);
    
    setseleph(SYS_GAL, 1); /* select Galileo ephemeris */

    /* set base staion position */
    if (opt->refpos<=POSOPT_RINEX&&opt->mode!=PMODE_SINGLE&&
        opt->mode!=PMODE_MOVEB) {
        for (i=0;i<6;i++) rtk->rb[i]=i<3?opt->rb[i]:0.0;
    }

    /* count rover/base station observations */
    for (nu=0;nu   <n&&obs[nu   ].rcv==1;nu++) ;
    for (nr=0;nu+nr<n&&obs[nu+nr].rcv==2;nr++) ;
    
    time=rtk->sol.time; /* previous epoch */
    
    /* rover position by single point positioning */
    if (!pntpos(obs,nu,nav,&rtk->opt,&rtk->sol,NULL,rtk->ssat,msg)) {
        errmsg(rtk,"point pos error (%s)\n",msg);
        
        if (!rtk->opt.dynamics) {
            outsolstat(rtk);
            return 0;
        }
    }

    if (time.time!=0) rtk->tt=timediff(rtk->sol.time,time);
    
    /* single point positioning */
    if (opt->mode==PMODE_SINGLE) {
        outsolstat(rtk);
        return 1;
    }
    /* suppress output of single solution */
    if (!opt->outsingle) {
        rtk->sol.stat=SOLQ_NONE;
    }
    /* precise point positioning */
    if (opt->mode>=PMODE_PPP_KINEMA) {
        pppos(rtk,obs,nu,nav);
        outsolstat(rtk);
        return 1;
    }
    /* check number of data of base station and age of differential */
    if (nr==0) {
        errmsg(rtk,"no base station observation data for rtk\n");
        outsolstat(rtk);
        return 1;
    }
    if (opt->mode==PMODE_MOVEB) { /*  moving baseline */
        
        /* estimate position/velocity of base station */
        if (!pntpos(obs+nu,nr,nav,&rtk->opt,&solb,NULL,NULL,msg)) {
            errmsg(rtk,"base station position error (%s)\n",msg);
            return 0;
        }
        rtk->sol.age=(float)timediff(rtk->sol.time,solb.time);
        
        if (fabs(rtk->sol.age)>TTOL_MOVEB) {
            errmsg(rtk,"time sync error for moving-base (age=%.1f)\n",rtk->sol.age);
            return 0;
        }
        for (i=0;i<6;i++) rtk->rb[i]=solb.rr[i];
        
        /* time-synchronized position of base station */
        for (i=0;i<3;i++) rtk->rb[i]+=rtk->rb[i+3]*rtk->sol.age;
    }
    else {
        rtk->sol.age=(float)timediff(obs[0].time,obs[nu].time);
        
        if (fabs(rtk->sol.age)>opt->maxtdiff) {
            errmsg(rtk,"age of differential error (age=%.1f)\n",rtk->sol.age);
            outsolstat(rtk);
            return 1;
        }
    }
    /* relative potitioning */
    relpos(rtk,obs,nu,nr,nav);
    outsolstat(rtk);
    
    return 1;
}
